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Abstract 

Emerging  technology  in  small  autonomous  flying  vehicles  requires  the  systems 
to  have  a  precise  navigation  solution  in  order  to  perform  tasks.  Where  available,  the 
Global  Positioning  System  (GPS)  is  an  excellent  solution  providing  sub-meter  level 
accuracy  and  drift  free  position  using  a  small  size  receiver  and  low  power  consumption. 
In  many  critical  environments,  such  as  indoors,  GPS  is  unavailable  necessitating  the 
development  of  supplemental  aiding  sensors  to  determine  precise  position.  Small 
scale  inertial  measurement  units  are  a  popular  solution,  but  suffer  from  significant 
error  drift  over  time.  To  improve  results,  additional  sensors  are  required. 

This  research  investigates  the  use  of  a  line-scanning  laser  radar  (LADAR)  as  a 
standalone  two  dimensional  position  and  heading  navigation  solution  and  sets  up  the 
device  for  augmentation  into  existing  navigation  systems.  A  fast  histogram  correlation 
method  is  developed  to  operate  in  real-time  on  board  the  vehicle  providing  position 
and  heading  updates  at  a  rate  of  10  Hz.  A  non-linear  quadrotor  simulator  generates 
realistic  flight  profiles  which  are  then  used  to  produce  LADAR  scans  as  measured  by 
a  sensor  on  the  vehicle.  These  simulations  are  then  compared  to  experimental  results 
collected  using  a  SICK  LD-OEM  1000. 

The  histogram  correlation  algorithm  applied  in  this  work  was  shown  to  success¬ 
fully  navigate  a  realistic  environment  providing  a  position  accurate  to  40  cm  after  1 
minute  though  additional  aiding  will  be  required  to  support  long  duration  flights. 
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Two  Dimensional  Positioning  and  Heading  Solution 
for  Flying  Vehicles  using  a  Line-Scanning  Laser  Radar 

(LADAR) 

I.  Introduction 

Recent  technology  has  pushed  the  development  of  unmanned  aerial  vehicles 
(UAV)  to  perform  increasingly  complicated  tasks.  These  tasks  require  accurate  knowl¬ 
edge  of  the  vehicle’s  position  and  attitude.  Larger  aircraft  are  capable  of  carrying 
complex,  albeit  expensive  inertial  navigation  systems  (INS)  with  ring  laser  gyros  and 
accelerometers  able  to  maintain  position  to  one  or  two  miles  even  after  several  hours 
of  operation  [39].  However  the  navigation  systems  of  micro  air  vehicles  (MAV),  a  class 
of  small  UAV,  do  not  have  the  resources  to  carry  such  systems  and  instead  must  use 
other  methods  for  position  information.  A  popular  selection  is  microelectromechan¬ 
ical  system  (MEMS)  devices  because  they  are  lightweight,  compact,  and  generally 
low  cost.  These  are  subject  to  much  larger  error  drift  rates  than  other  conventional 
systems.  The  global  positioning  system  (GPS)  provides  meter-level  accuracy  and  can 
significantly  reduce  the  error  drift  of  an  onboard  INS  [29].  Unfortunately  GPS  is  not 
always  available  in  critical  environments  due  to  the  restrictions  of  the  system.  Satel¬ 
lite  reception  necessary  to  calculate  a  position  is  not  available  indoors,  underground, 
underwater,  or  even  in  harsh  terrain  such  as  dense  forests  and  canyons.  To  overcome 
these  issues  and  to  attempt  to  limit  the  error  growth,  additional  sensors  are  placed 
on  MAVs  to  provide  aiding  information  to  the  onboard  navigation  system.  Sensors 
may  also  be  added  when  GPS  is  available  as  they  are  able  to  observe  higher  frequency 
dynamics  where  GPS  captures  low  frequency  dynamics.  Together  they  are  able  to 
provide  a  better  navigation  solution. 

This  thesis  investigates  the  characteristics  and  limitations  of  an  onboard  line- 
scanning  radar  (LADAR)  when  operating  in  a  3  dimensional  environment.  This  tech- 
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nology  is  also  referred  to  as  Light  Detection  and  Ranging  (LiDAR).  These  devices 
measure  a  range  to  an  object  often  using  infrared  laser  which  is  emitted,  reflects  off 
an  object,  and  is  then  detected  by  the  unit.  Line-scanning  LADAR  measure  numer¬ 
ous  points  around  a  circular  field  of  view,  often  using  a  motor  to  spin  a  mirror  to 
reflect  the  laser.  Currently  these  are  primarily  used  for  navigation  using  Simultane¬ 
ous  Localization  and  Mapping  (SLAM)  techniques.  These  algorithms  often  require 
extensive  processing  power  and  data  is  post  processed. 

To  avoid  these  limitations,  a  LADAR  unit  uses  scan  matching  techniques  to 
investigate  an  independent  2  dimensional  position  and  heading  solution  suitable  for 
use  on  a  flying  vehicle.  To  evaluate  the  potential  of  this  system,  several  assumptions 
of  the  operation  environment  are  made  and  the  capabilities  expanded  as  appropriate. 
Initially,  the  simulated  environment  is  an  indoor  man  made  structure  where  there  are 
parallel  and  perpendicular  walls  without  obstructive  objects.  These  results  are  then 
compared  to  experimental  data  collected  in  a  realistic  and  cluttered  environment. 

The  remainder  of  the  thesis  is  organized  as  follows:  Chapter  2  covers  background 
information  on  the  navigation  and  control  systems  of  small  flying  vehicles  along  with 
previous  research  work  performed.  Chapter  3  details  the  complete  problem  solving 
approach  including  simulations  used  to  evaluate  LADAR  as  an  aiding  sensor.  Chap¬ 
ter  4  provides  results  and  quantitative  analysis  of  the  simulation.  Chapter  5  draws 
conclusions  and  recommends  future  research. 
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II.  Background 


This  chapter  presents  background  information  about  micro  air  vehicle  operation, 
the  motivations  for  MAV  flight,  the  navigation  and  control  systems  utilized 
by  these,  specifically  the  limitations  of  these  systems  derived  from  the  vehicle,  and 
various  localization  and  mapping  techniques  used. 

2.1  Motivation  for  MAV  Flight 

Micro  air  vehicles  provide  new  opportunities  and  applications  to  unmanned  ve¬ 
hicle  operation.  Examples  for  the  motivations  of  civilian  operation  of  these  vehicles 
include  remote  search  and  rescue  in  hazardous  areas,  observation  of  difficult  to  ob¬ 
serve  construction  sites  such  as  bridges,  or  mobile  weather  observation  platforms  [16]. 
MAVs  could  also  enable  military  applications  such  as  autonomous  surveillance  or 
perform  tasks  in  hostile  areas. 

2.2  MAV  Navigation  Systems 

Large  aircraft,  such  as  airliners  or  military  jets,  are  capable  of  carrying  incredibly 
accurate  self-contained  yet  expensive  navigation  system.  Through  the  use  of  ring 
laser  gyros  and  accelerometers  the  INS  can  determine  it’s  position  within  0.6  nautical 
miles  after  one  hour  or  better  [39].  However,  these  systems  are  not  feasible  on  small 
air  vehicles,  not  only  because  of  cost,  but  also  size  and  weight.  Smaller  versions 
of  an  INS  often  utilize  MEMs  devices  which  are  nowhere  near  as  accurate,  and  drift 
significantly  [39].  For  very  short  flights  (less  than  a  few  minutes)  they  may  be  suitable, 
but  problems  arise  quickly  because  of  their  large  error  bias  and  drift  rates. 

2.3  Aiding  Sensors 

The  robotics  community  has  been  a  driving  force  for  localization  and  mapping 
techniques  for  many  years.  An  investigation  into  a  wide  variety  of  sensors  successfully 
implemented  onto  vehicles,  along  with  the  methods  for  navigation  aiding,  can  be  found 
in  a  LIniversity  of  Michigan  report  for  the  Oak  Ridge  National  Lab  D&D  program  [7]. 
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While  many  of  these  techniques  are  optimized  for  land  based  vehicles,  the  technology 
can  be  adapted  to  flying  vehicles.  These  vehicles  also  have  the  capability  for  the  vehicle 
to  act  as  a  gimbal  increasing  a  sensors  potential  without  the  addition  of  additional 
hardware  such  as  a  servo. 

As  previously  mentioned,  the  nature  of  MAV  flight  restricts  the  type  of  equip¬ 
ment  and  sensors  able  to  be  flown.  Issues  include  limited  payload,  electrical  power, 
and  processing  capabilities  available.  Beyond  these,  operational  environment  also  in¬ 
fluences  sensor  selection.  A  desired  sensor  would  not  depend  on  the  situation  the 
vehicle  is  deployed,  such  as  depending  upon  ambient  light  which  is  a  problem  with 
some  vision  based  systems.  It  should  also  be  non-destructive  in  any  way.  This  in¬ 
cludes  physical  contact  with  surrounding  structures,  or  even  radiation  emitted  from 
active  sensors,  such  as  vision  damaging  lasers.  In  military  applications  passive  sensors 
are  preferred  because  of  they  are  not  detectable  to  the  enemy. 

Accuracy  of  measurement  systems  is  often  a  motivating  factor  in  sensor  selec¬ 
tion.  This  is  usually  related  to  the  wavelength  of  the  signal  used.  Sonar  or  ultrasonic 
sensors,  such  as  the  LV-MaxSonar-EZO,  use  42  KHz  signals  to  a  determine  a  range 
at  a  resolution  of  1  inch  [27].  LADAR  systems  generally  use  significantly  higher  fre¬ 
quencies  in  the  infrared  spectrum.  A  stationary  non  line-scanning  device  used  for 
surveying  can  provide  mm  level  accuracy  with  the  appropriate  processing  [15]. 

Current  rotating  3D  LADAR  systems  are  heavy  and  produce  massive  amounts 
of  data,  both  a  hindrance  on  MAV  systems.  A  common  example  of  this  type  of  sensor, 
the  Velodyne  HDL-64E,  uses  64  lasers  to  produce  a  360  degree  horizontal  and  26.8 
degree  vertical  field  of  view  with  an  angular  resolution  of  0.09  degrees,  and  operates  at 
a  rate  between  5  and  15  Hz.  This  results  in  over  1.3  million  points  per  second  which 
cannot  currently  be  processed  onboard  an  MAV.  This  device  is  incredibly  popular  in 
the  DARPA  Urban  Challenge  in  which  autonomous  vehicles  must  navigate  roads  and 
obstacles  [12,37].  A  new  version  by  Velodyne  suits  itself  more  for  flying  vehicles.  The 
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HDL-32E  LiDAR  sensor  measures  5.9  by  3.4  inches  and  weighs  less  than  3  pounds 
with  production  units  shipping  in  the  fall  of  2010  [24], 

Flash  LADAR  systems  are  a  smaller  alternative  and  produce  a  grid  of  depth 
measurements.  For  example,  the  Advanced  Scientific  Concepts  TigerEye  3D  flash 
LiDAR  camera  creates  a  128  x  128  grid  at  a  rate  of  30  Hz  with  varying  held  of  views 
and  ranges.  A  3  degree  held  of  view  yields  up  to  1100  m  range  measurements  where  as 
a  45  degree  held  of  view  has  a  maximum  range  of  60  m  [9].  A  similar  device,  the  Mesa 
Imaging  SR-4000  was  used  to  ht  planes  in  an  environment  and  assisted  to  constrain 
drift  in  an  IMLI  [19]. 

Often,  it  is  common  to  implement  various  sensors  together  to  take  advantage 
of  their  capabilities.  For  example  a  line-scanning  LADAR  and  a  camera  are  used  to 
successfully  navigate  a  cluttered  room  [4], 

This  thesis  fully  characterizes  the  line-scanning  LADAR  as  a  single  sensor  so¬ 
lution  to  determine  a  2  dimensional  position  and  heading.  It  expands  on  a  current 
histogram  correlation  frame  matching  technique  and  investigates  methods  to  optimize 
for  operation  in  a  3  dimensional  environment.  This  increases  the  hexibility  when  im¬ 
plemented  on  future  systems. 

A  line-scanning  radar  was  chosen  because  it  hts  within  the  limitations  of  a  small 
hying  vehicle  and  has  extensive  development  in  the  ground  based  robotic  vehicle  held 
which  is  detailed  later  in  this  chapter.  The  most  common  unit  used  in  many  mobile 
robotics  applications  is  the  SICK  Laser  Measurement  Sensor  LMS-200  series.  It  is  a 
180  degree  scanner  with  a  range  of  80  m.  SICK  has  a  few  other  models  including  a 
360  degree  variant,  the  LD-OEM  1000,  which  measures  at  a  rate  of  15  Hz  and  a  range 
of  over  100  meters.  A  higher  speed  scanner,  the  LMS-100,  operates  up  to  50  Hz,  but 
sacrihces  range  to  18  meters  and  is  only  able  to  measure  up  to  a  270  degree  held  of 
view.  Another  company  with  competing  scanning  laser  range  finders  is  Hokuyo.  The 
units  used  in  robotics  and  hying  vehicles  are  considerably  smaller  and  lighter  but  suffer 
from  reduced  range  and  held  of  view.  A  commonly  used  scanner  which  was  acquired 
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by  the  Advanced  Navigation  Technology  (ANT)  Center  for  another  quadrotor,  the 
URG-04LX,  measures  a  225  degree  window  inside  the  270  degree  available  area  at  10 
Hz  to  a  range  of  4  meters  but  weighs  only  5  ounces. 

Similar  work  using  line-scanning  laser  radar  has  shown  promising  results  in  the 
aiding  performance  of  a  2D  LADAR  in  a  3D  environment  [35].  Currently  this  work 
assumes  a  man  made  structure  and  performs  plane  fitting  to  the  environment.  This 
thesis  provides  the  basis  for  a  real  time  relative  positioning  method  which  avoids 
computationally  restrictive  SLAM  elements  which  will  be  detailed  in  Section  2.4.3. 

A  specific  sensor,  the  SICK  LD-OEM  1000,  was  chosen  for  initial  experimenta¬ 
tion  and  simulation  to  analyze  the  technology  currently  available.  The  critical  system 
specifications  for  this  are  shown  in  Table  2.1  [2],  Although  this  particular  sensor  is 
too  large  for  most  MAVs,  the  algorithms  developed  will  apply  equally  to  current  and 
future  devices  which  are  smaller. 

A  common  approach  for  extracting  3D  data  from  a  line-scanning  radar  is  to 
use  a  servo  to  rotate  the  unit  on  its  axis  [6].  Other  ideas  include  placing  mirrors  in 
front  of  the  scanner  to  redirect  the  beam  [30].  A  similar  method  which  does  not  use 
LADAR  implements  four  single  direction  ultrasonic  and  two  IR  ranging  sensors.  It 
rotates  in  circles  to  generate  a  scan  similar  to  a  line-scanning  LADAR  and  uses  this 
data  to  proceed  to  the  center  of  the  room  and  map  its  shape  [10]. 

2-4  Self  Localization 

Before  detailing  the  specific  techniques  currently  used  with  LADAR  sensors, 
necessary  background  topics  of  robotic  navigation  and  broad  sensor  data  extraction 
methods  will  be  discussed. 

2-4-1  Reference  Frames.  Humans  usually  interpret  navigation  as  to  where 
an  object,  such  as  the  quadrotor,  is  located  in  a  room,  or  where  they  find  their 
car  traveling  on  a  map.  This  is  defined  as  the  navigation  frame.  When  a  sensor  on  a 
vehicle  makes  a  measurement  it  is  unaware  of  its  location  in  the  navigation  frame,  and 
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Table  2.1:  LD-OEM  1000  Universal  Laser  Distance  Scanner  Technical  Data 


Parameter 

Value 

Useful  Scanning  Angle 

360° 

Angular  Resolution 

0.125° 

Scanning  Frequency 

5-15  Hz  ±  5%  increments  of  1  Hz 

Measurement  Resolution 

3.9  mm 

Beam  Divergence  Angle 

2.5  mrad  (0.143°) 

Laser  Diode 

IR  (A  =  905  nm) 

Measurement  Range  (reflectivity  of  material) 

5%  Relection  (Black) 

0.5  -  24  m 

20%  Reflection 

0.5  -  50  m 

90%  Reflection 

0.5  -  100  m 

100%  Reflectors 

0.5  -  250  m 

only  has  a  reference  to  how  it  is  mounted  on  the  vehicle.  This  frame  is  designated  the 
body  frame.  Additional  reference  frames  are  often  necessary.  The  required  reference 
frames  used  in  this  thesis,  along  with  calculations  and  transformations  are  detailed  in 
Chapter  3. 

2-4-2  Line  and  Feature  Extraction.  Urban  environments  often  provide  struc¬ 
tures  with  lines  which  are  useful  landmarks  to  be  used  for  navigation.  Early  work 
included  fitting  line  segments  to  environments  using  LADAR  [38].  Numerous  other 
methods  have  been  developed  and  a  few  of  the  algorithms  are  compared  by  Nguyen 
et  al  [31].  These  methods  include  an  incremental  method  where  points  are  added  to 
a  line  until  the  next  consecutive  point  no  longer  satisfies  specified  conditions.  Others 
use  a  sliding  window  of  points  to  fit  line  segments  to  points.  Once  all  segments  are 
generated  colinear  segments  can  be  merged  into  lines. 

Vision  based  systems  have  more  information  available  due  to  a  full  frame  of  color 
information  but  can  use  similar  techniques.  Scale-Invariant  Feature  Transform  (SIFT) 
is  commonly  used  to  detect  features  in  an  image.  After  processing  using  this  algorithm 
high-contrast  points  in  an  image  are  determined  and  can  be  matched  across  multiple 
images  to  produce  estimates  of  movement  and  attitude  change.  Similar  algorithms 
such  as  the  Canny  edge  detection  method  can  be  applied  to  detect  lines. 
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2-4-3  SLAM.  Simultaneous  Localization  and  Mapping  (SLAM)  is  a  concept 
used  by  autonomous  robots  to  attempt  to  build  a  map  of  an  unknown  environment 
as  it  navigates.  The  robot  must  have  an  accurate  model  of  its  system  dynamics  along 
with  sensors  to  provide  observability  to  features  and  landmarks  in  the  area.  These 
features,  such  as  walls  or  stationary  objects  are  detected  by  a  sensor  are  placed  into  a 
map  according  to  the  robots  current  estimated  position,  often  estimated  using  odom- 
etry.  LADAR,  vision  based  cameras,  or  sonar  devices  can  be  used  to  extract  features. 
When  a  robot  returns  to  a  location  previously  mapped  it  is  able  to  recognize  identical 
features  and  adjust  its  position  estimate  bounding  the  growth  in  errors  developed  over 
time  such  as  those  cause  by  wheel  slip,  or  measurement  drift  in  an  IMU. 

2-4-4  Frame  to  Frame  Correlation.  Avoiding  computation  and  memory  stor¬ 
age  elements  required  in  SLAM,  frame  to  frame  correlation  is  used  as  an  alternative 
method  to  processing  vision  or  LADAR  based  measurements.  Consecutive  sensor 
measurements  can  be  compared  to  determine  movement  or  attitude  change  in  real 
time  clue  to  reduced  computational  requirements.  Disadvantages  to  this  approach 
include  unbounded  error  growth.  Complex  correlation  methods  using  probabilistic 
approaches  requiring  intense  computation  were  implemented  in  real-time  using  paral¬ 
lel  processing  of  graphics  processing  units  and  outperformed  many  other  correlation 
methods  [32], 

2-4-5  Histogram  Correlation.  An  alternative  to  complex  and  computation 
hungry  SLAM  or  probabilistic  algorithms  takes  a  different  approach.  The  histogram 
method  is  a  frame  to  frame  correlation  technique  which  uses  two  consecutive  LADAR 
scans  to  find  the  rotation  and  translation  of  the  vehicle  [42],  This  is  explained  in 
detail  in  Chapter  3.  A  brief  synopsis  follows: 

The  scanner  data,  provided  in  polar  coordinates,  is  first  mapped  to  the  Cartesian 
body  frame.  Each  set  of  consecutive  points  forms  a  line  segment  and  its  angle  is 
calculated  in  relation  to  the  scanner  (body)  frame.  These  angles  are  then  collected 
into  a  histogram  with  bins  of  the  calculated  angle.  When  operating  in  man  made 


environments  where  one  can  assume  near  perpendicular  and  parallel  walls  this  results 
in  significant  peaks  in  the  histogram. 

In  the  absence  of  translation  and  measurement  noise,  two  scans  will  be  identical 
except  for  a  phase  shift.  Noise  and  movement  will  cause  subtle  differences  but  per¬ 
forming  a  correlation  between  the  two  scans  will  result  in  the  rotation  angle.  There 
are  known  cases  such  as  a  large  rotation  which  may  fail  correct  detection  with  this 
method,  but  there  are  ways  to  mitigate  this.  One  method  is  to  limit  the  search 
window  based  on  an  additional  sensor’s  estimate  (MEMs  gyro),  or  using  different 
algorithms  [22], 

Upon  calculation  of  the  vehicle’s  rotation  between  the  consecutive  frames  two 
additional  histograms  are  formed  to  determine  translation  in  two  orthogonal  direc¬ 
tions.  First,  the  measurements  of  one  scan  are  rotated  by  the  calculated  rotation 
angle  so  that  the  scans  lie  in  the  same  orientation  in  the  navigation  frame.  Then,  an 
x-direction  and  y-direction  histogram  are  used  to  determine  a  Ay  and  Ax  translation. 
These  correspond  to  movement  of  the  vehicle  in  the  body  frame  which  can  then  be 
transformed  to  the  navigation  frame  [42], 

Advantages  to  the  histogram  approach  are  avoiding  costly  line  or  feature  de¬ 
tection  algorithms  while  mitigating  the  effects  of  sensor  noise.  Also,  moving  objects 
in  the  measurement  area  do  not  degrade  performance  significantly.  This  method  was 
shown  to  assist  in  the  construction  of  accurate  maps  in  real  time,  using  a  Pentium 
III-600  computer  [34], 

One  proposed  improvement  to  the  algorithm,  especially  in  cases  where  there 
is  a  large  rotation  between  frames,  uses  vector  histogram  matching  [22],  Another 
modification  successfully  mapped  an  unknown  outdoor  environment  [8] .  Other  devel¬ 
opments  of  these  algorithms  claim  to  greatly  improve  calculation  speed,  with  future 
work  laid  out  to  improve  accuracy  of  scan  matches  [33]. 

This  thesis  investigates  the  use  of  a  method  originally  designed  to  use  onboard 
driving  robotic  vehicles.  These  vehicles  have  access  to  odometry  information  which 
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assists  overall  performance  with  an  independent  estimate  of  the  vehicle’s  dynamics. 
A  different  LADAR  than  often  used  with  this  algorithm  utilizes  a  higher  resolution 
and  360  degree  field  of  view  to  apply  a  similar  algorithm  to  determinate  a  relative 
positioning  solution.  The  additional  observability  and  range  measurements  from  the 
scanner  improve  the  devices  capability  to  produce  a  standalone  position  and  heading 
solution.  The  research  is  then  extended  to  consider  the  effects  of  small  attitude 
changes  as  are  typical  of  rotary  aircraft. 

2.5  Aiding  Navigation  with  Sensors 

2.5.1  Kalman  Filters.  The  presence  of  noise  and  inaccuracies  in  all  sensor 
measurements  motivate  the  use  of  a  Kalman  filter  in  aircraft  navigation  systems.  It 
produces  optimal  estimates  of  the  vehicle  states  by  using  a  three  step  process.  First, 
the  system  states  are  estimated  using  a  model,  then  the  expected  uncertainty  in  these 
estimates  is  calculated,  and  finally  the  incoming  measurements  are  weighed  appro¬ 
priately  before  incorporating  them  into  the  state  estimate.  If  implemented  correctly, 
this  process  also  helps  capture  some  of  the  unknown  errors  associated  with  drifts  and 
biases  in  the  IMU  [28]. 
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III.  The  Histogram  Correlation  Method 


This  chapter  outlines  the  process  used  to  transform  sensor  data  into  a  navigation 
solution.  First,  a  simulation  was  developed  in  two  dimensions  to  observe  how 
the  different  algorithms  used  by  mobile  robots  perform.  After  selection  of  an  algorithm 
the  simulation  will  be  expanded  into  3  dimensions  where  imitated  flight  trajectory 
and  attitude  information  can  be  implemented.  These  allow  the  generation  of  realistic 
LADAR  data  scans  identical  to  those  produced  on  the  vehicle.  Different  processing 
methods  can  then  be  applied  to  these  laser  scans  and  evaluated  in  performance  of 
determining  a  position  and  heading. 

3.1  Scanner  Data 

Line-scanning  LADAR  systems  measure  only  a  distance  from  the  scanner  to  the 
first  object  in  it’s  path  using  radio  frequencies  usually  in  the  ultraviolet,  visible,  or 
infrared  range.  The  two  most  common  methods  are  using  time  of  flight  and  phase 
based  measurements.  In  a  time  of  flight  system  the  distance  is  calculated  using 
Equation  3.1: 


distance 


Speed  of  Light  x  Time  of  Flight 
2 


(3.1) 


These  sensors  measure  data  up  to  1500  m  and  are  accurate  to  1-10  cm.  Phase 
based  systems  produce  measurements  accurate  to  a  few  mm  but  arc  limited  to  a  range 
of  70  m  [15]. 

To  increase  a  ranging  sensor’s  usefulness  a  motor  is  used  to  spin  some  element 
of  the  scanner,  usually  the  mirror,  allowing  measurements  to  be  collected  up  to  a 
360  degree  field  of  view.  Typical  accuracy  of  the  systems  used  on  mobile  robots, 
depending  on  the  reflectivity  of  the  object  and  the  distance  between  the  object  and 
the  scanner,  is  a  few  to  ten  centimeters  with  minimal  angle  uncertainty.  In  [5]  these 
errors  are  detailed  in  great  length  regarding  the  SICK  LMS-200.  Similar  analysis  is 
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not  available  for  the  LD-OEMIOOO  but  similar  errors  are  expected  to  be  present  in 
most  line-scanning  LADAR  devices. 

Data  generated  by  a  LADAR  unit  is  in  polar  coordinates  consisting  of  an  angle, 
6,  and  a  range,  r.  The  angle  is  rarely  transmitted  by  the  scanner  but  relics  on  the 
user  to  account  for  this.  A  simple  transformation  is  used  to  convert  it  into  Cartesian 
coordinates  is: 


x  —  r  sin(0) 
y  =  r  cos(0) 


(3.2) 


where  x  and  y  are  the  Cartesian  coordinates,  r  is  the  measurement  range,  and  6  is 
the  angle. 


The  SICK  LD-OEM  1000  unit  has  the  capability  to  utilize  a  user  defined  scan 
area,  resolution,  and  scan  rate.  These  are  restricted  due  to  hardware  capabilities  and 
limited  by  the  pulse  frequency  of  the  laser  diode  in  the  unit  to  avoid  damage  caused 
by  heat.  The  maximum  pulse  frequency  is  limited  to  14.4  kHz  with  a  maximum 
mean  pulse  frequency  over  one  scan  of  10.8  kHz.  For  this  application  a  constant 
measurement  resolution  across  the  whole  360  degree  area  of  view  will  be  used.  A 
common  setting  for  this  type  of  application,  and  the  one  used  for  the  majority  of  this 
work,  is  1/2  degree  resolution  scanning  a  full  360  degree  revolution  at  a  rate  of  10 
Hz  [1], 

When  using  LADAR  data  during  the  operation  of  a  vehicle,  usually  one  complete 
revolution  of  the  scanner  is  used  as  a  single  piece  of  data.  This  may  be  compared 
to  another  complete  scan,  or  by  using  a  SLAM  based  approach  where  each  scan  is 
linked  to  an  environment  map  and  features  are  tracked  across  the  full  scan  history. 
To  investigate  the  performance  of  LADAR  operating  in  a  dynamic  flight  environment 
a  simulation  using  MATLAB®  was  developed.  Performance  in  the  simulation  is  then 
compared  to  data  collected  in  a  real  environment. 
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3.2  Simulation 


3.2.1  Background  Setup.  Using  a  simulation  for  initial  investigation  of 
LADAR  performance  provides  the  opportunity  to  test  various  environments  and  sen¬ 
sor  capabilities  before  unknown  real  world  effects  degrade  performance.  The  first  step 
was  to  develop  a  technique  to  represent  an  environment.  In  two  dimensions  a  line 
segment  is  the  simplest  feature  to  implement  by  designating  two  coordinate  points  in 
the  navigation  frame.  For  simplicity,  the  first  two  walls  of  a  100  meter  square  room 
are  shown  in  Figure  3.1.  Another  shape  of  interest  is  a  circle.  This  may  represent  an 
obstruction  in  the  room  such  as  a  support  pole,  or  even  a  rough  estimate  for  a  person 
standing  in  the  room.  To  represent  this  a  coordinate  point  and  a  radius  defines  a 
circle.  Besides  the  room  structure  to  represent  various  environments,  a  scanner  struc- 


Figure  3.1:  Simple  Setup  of  an  Experimental  Room  in  Simulation.  Two  walls  of 
a  100  meter  square  room.  The  coordinate  pairs  for  the  line  segments  are  (0,0)  to 
(0,100),  and  (0,100)  to  (100,  100).  The  circle  in  the  center  is  at  point  (50,50)  with  a 
radius  of  10. 

ture  was  developed  to  easily  change  sensor  characteristics  such  as  range  or  noise  on 
the  measurements.  The  primary  variables  are  the  range  p,  measurement  noise,  crn, 
and  resolution  (angular  step  width).  When  movement  of  the  scanner  is  implemented 
the  scan  frequency  of  the  unit  is  also  necessary. 
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One  additional  structure  is  required  to  represent  the  vehicle  and  measurement 
system  in  the  body  frame.  To  perform  a  scan  the  vehicle  state  must  be  defined  with 
an  x  and  y  position  (PXl  Py ),  and  the  vehicle  heading  (ipv).  Velocities  and  angular 
rates  are  necessary  for  moving  vehicles.  An  alternative  is  to  externally  generate  the 
flight  profile  and  vehicle  states  over  time  and  then  import  these  into  the  simulation 
to  generate  scan  data.  If  the  LADAR  is  not  placed  at  the  center  of  the  vehicle  a 
vector  addition  must  be  performed  to  account  for  the  offset.  Likewise,  if  the  scanner 
and  body  heading  are  not  the  same,  the  difference  must  be  accounted  for.  These 
cases  may  introduce  degraded  performance  as  a  rotation  of  a  vehicle  will  cause  an 
undesirable  LADAR  position  trajectory  when  the  scanner  does  not  move  in  a  straight 
line  during  the  scan.  For  simplicity,  the  scanner  position  will  be  collocated  with  the 
origin  of  the  body  frame. 

3.2.2  Scan  Calculation.  With  the  environment,  scanner  characteristics,  and 
vehicle  state  defined,  the  LADAR  is  ready  to  perform  a  simulated  scan.  The  sensor 
heading  defines  where  the  first  measurement  is  taken  (0  degrees  in  the  body  frame). 
The  LD-OEM  1000  rotates  counterclockwise,  so  its  heading  is  opposite  the  vehicle, 
where  a  counterclockwise  rotation  is  positive  in  magnitude.  The  laser  measurement 
is  modeled  as  a  line  segment  from  the  vehicles  current  position,  to  the  point  at  it’s 
maximum  detection  range,  at  the  current  angle.  This  is  detailed  in  Figure  3.2  and 
calculated  as  follows: 


{xl,y  1)  =  {Px,Py) 

(x2,y2)  =  (Px  +  pcos(^m),  Py  +  p sin(^m)) 


(3.3) 


where  (xl,yl)  is  the  vehicle  position  and  (x2,y2)  is  the  maximum  measurement  point 
of  the  scanner  at  its  current  orientation,  i/j v,  the  vehicle’s  heading,  and  ips  the  scanner 
current  angle,  are  added  together  to  determine  the  measurement  angle. 


Next,  this  scanner  line  segment  must  be  checked  to  see  if  a  measurement  occurs. 
It  is  checked  for  a  collision  against  all  elements  of  the  environment  structure.  To 
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Figure  3.2:  Simulated  Laser  Range  Measurement.  This  figure  represents  a  simu¬ 

lated  measurement  from  a  LADAR  unit  in  the  previously  defined  room  at  a  position 
of  (10,10).  The  vehicle  heading  of  -45  degrees,  Cv,  is  added  to  the  scanner  heading, 
ij)s,  of  10  degrees,  and  the  measurement  calculates  a  line  segment  in  the  navigation 
frame  from  the  unit  to  its  range  of  50  meters. 

perform  the  necessary  calculations  for  two  dimensional  line  to  line,  and  line  to  circle 
intersections,  Kevin  Lindsey’s  JavaScript  code  was  ported  to  MATLAB  [25].  If  an 
intersection  was  found  the  simulation  keeps  track  of  the  distance  and  coordinate 
point  that  this  occurs.  After  checking  all  possible  objects  in  the  environment  only  the 
shortest  distance  is  recorded.  If  there  is  a  sensor  noise  attribute  it  is  added  at  this 
point  as  a  Gaussian  random  variable  to  the  measurement  range,  and  it’s  coordinate 
pair  is  also  recorded.  This  method  of  adding  noise  assumes  perfect  measurement 
angle  alignment  in  the  sensor.  Figure  3.3  shows  a  simulated  scan  of  the  environment 
detailed  above.  This  noise  method  for  adding  measurement  noise  was  verified  with 
real  data,  and  is  detailed  in  the  next  section. 

3.2.3  True  Scanner  Data  Comparison.  To  verify  the  simulated  laser  scan 
generation  method  the  LD-OEM  1000  was  setup  to  take  measurements  in  the  ANT 
Center  at  the  Air  Force  Institute  of  Technology  (AFIT).  The  walls  of  the  ANT  Center 
were  measured  and  mapped  to  the  nearest  meter.  The  room  is  a  horseshoe  shape  due 
to  a  temporary  office  setup  with  portable  walls  in  the  center.  It  also  has  long  hallways 
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Figure  3.3:  Full  Laser  Scan  Measurement.  This  figure  represents  a  laser  scan  of  the 
vehicle  at  position  (10,10)  and  every  5  degree  measurement  shown  in  red.  A  black  x 
represents  the  measured  point  of  the  intersection  with  a  Gaussian  noise  of  2  cm  which 
is  not  noticeable  at  this  scale. 

just  outside  the  entrance.  To  provide  a  simple  model  of  these  hallways  they  extend 
100  meters  in  both  directions  and  are  included  in  the  top-down  simulation  map  as 
shown  in  Figure  3.4. 
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Figure  3.4:  ANT  Center  Simulation  Layout 

The  LD-OEM  1000  online  data  sheet  claims  a  1-er  statistical  error  of  ±25  mm, 
a  systematic  error  of  ±38  mm,  and  a  resolution  of  3.9  mm.  This  resolution,  where 
the  discrete  measurements  can  occur,  is  visible  in  Figure  3.5(a).  Since  no  truth 
measurements  are  available  at  this  accuracy  only  the  precision  of  the  scanner  can  be 
analyzed  using  this  method.  The  simulated  scans,  Figure  3.5(b),  were  performed  with 
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an  additive  white  Gaussian  noise  of  strength  38  mm.  The  truth  scan  performed  much 
better  than  expected  compared  to  the  errors  quoted  on  the  datasheet.  The  average 
calculated  standard  deviation  over  100  scans,  calculated  for  each  measurement  point, 
was  0.91  cm  for  the  experimental  data.  For  the  simulated  laser  scans  the  noise  was 
near  expected  at  3.77  cm.  This  indicates  that  the  sensor  may  perform  better  than 
the  original  expectation  of  a  noise  of  3.8  cm.  The  accuracy  is  said  to  degrade  with 
greater  measurement  distance  from  the  scanner  which  can  attributed  to  the  systematic 
error  [2], 

3.2.4  Reference  Frames.  A  human  operating  an  air  or  land  vehicle  is  likely 
to  be  interested  in  its  position  in  the  environment,  or  navigation  reference  frame, 
whether  it  be  determining  which  street  they  are  driving  on,  or  if  they  are  operating 
an  aircraft  in  a  restricted  airspace.  In  these  cases  the  operator  is  able  to  use  visual 
cues,  maps,  or  other  devices  to  aid  in  the  determination  of  the  approximate  location. 
An  autonomous  vehicle  is  unable  to  use  these  and  must  use  sensors  to  approximate 
its  position.  Most  sensors  operate  in  the  body  reference  frame  and  have  minimal 
knowledge  of  the  navigation  frame.  One  example,  a  forward  looking  camera  mounted 
on  the  nose  of  an  aircraft  has  a  limited  observability  of  the  nav  frame  at  any  time, 
but  it  does  have  a  constant  field  of  view  in  relation  to  the  body  frame.  An  exception 
to  this  is  a  GPS  device  which  provides  a  position  solution  in  the  navigation  frame, 
but  at  the  same  time  offers  limited  information  in  the  body  frame  such  as  aircraft 
attitude.  In  order  to  perform  calculations  across  reference  frames  a  Direction  Cosine 
Matrix  (DCM)  is  used.  This  is  time  dependent  and  changes  with  the  vehicle’s  state. 
These  calculations  are  further  detailed  in  Section  3.2.7.  The  body  frame  of  a  flying 
vehicle  is  often  defined  by  the  x-axis  going  from  the  center  of  the  body  out  the  nose, 
the  y-axis  points  through  the  right  wing,  and  to  complete  a  right  handed  coordinate 
system  the  z-axis  points  down.  The  navigation  frame  used  in  this  thesis  will  consist  of 
a  common  East,  North,  Up,  or  ENU,  coordinate  frame.  Another  option  is  the  North, 
East,  Down,  or  NED,  frame.  These  axes  are  defined  in  a  specific  order,  (x,y,z),  to 
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7.5 


- Measured  Scans 

- Mean+-c 


7.3 


Measurement  Index 


(a)  100  Stationary  Scans  of  LD-1000  Laser  Rangefinder  with  Mean  and  Standard  Deviation 
Shown 


(b)  100  Simulated  Stationary  Scans  at  a  Noise  of  38  mm  with  Mean  and  Standard  Devi¬ 
ation  Shown 

Figure  3.5:  Statistical  Analysis  of  Laser  Scan  Accuracies 

be  consistent  with  orthogonal  right-hand  coordinate  frames.  Vehicles  which  fly  faster 
or  further  during  the  duration  of  the  flight  use  another  common  reference  frame,  the 
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World  Geodetic  System  1984,  (WGS84)  which  is  in  the  class  of  reference  systems 
called  Earth  Centered,  Earth  Fixed  (ECEF)  where  the  origin  is  the  center  of  mass 
of  the  Earth,  and  a  point  is  defined  by  a  point  on  the  Earth  in  latitude,  longitude, 
and  altitude.  This  is  the  frame  that  GPS  provides  a  position  solution  [29].  Figure  3.6 
shows  the  relation  of  the  reference  frames  necessary  for  this  thesis. 


x 


Navigation 

Frame 


Figure  3.6:  Reference  Frames  Necessary  to  Operate  LADAR  on  a  Vehicle  in  a 

Navigation  Frame 

In  this  thesis  the  measurement  frame,  the  frame  in  which  the  LADAR  takes 
measurements,  is  defined  to  be  collocated  with  the  body  frame.  Otherwise,  transfor¬ 
mation  between  the  navigation  and  measurement  frame  would  be  accomplished  using 
a  DCM  from  the  navigation  to  body  frame  followed  by  a  DCM  from  the  body  to 
measurement  frame,  and  vice  versa  for  the  opposite  transformation. 

3.2.5  Simulation  in  3D.  Upon  confirming  that  the  simulated  laser  scans 
suitably  matched  the  scanner  it  was  necessary  to  expand  into  a  flying  (3D)  envi¬ 
ronment.  First,  the  ANT  Center  needed  to  be  expanded  and  represented  in  three 
dimensions.  The  existing  layout  was  used  for  the  walls  and  hallways.  Instead  of  being 
represented  as  a  line,  each  wall  was  defined  as  an  infinite  planar  surface.  To  define 
its  limits  another  matrix  was  stored  into  the  environment  structure  accounting  for 
the  minimum  and  maximum  x,  y,  and  z  values  for  each  wall  which  is  referenced  when 
checking  for  a  measurement  intersection.  With  the  new  measurement  consisting  of 
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checking  between  a  line  segment  and  a  plane  a  3D  geometry  toolbox,  geom3d,  was 
implemented  for  simplicity  and  accuracy  [23].  To  complete  the  environment  structure 
of  the  ANT  center,  a  floor  and  ceiling  were  placed  as  a  flat  plane  3  meters  apart. 

The  next  step  is  to  generate  a  simulated  flight  profile,  ft  is  now  necessary  to 
include  additional  vehicle  states  consisting  of  the  aircraft’s  roll  and  pitch  angles,  along 
with  an  altitude. 

The  necessary  state  vector  to  place  the  aircraft  into  the  simulation  at  each  time 
a  range  measurement  is  taken  to  generate  simulated  scans  is: 


Xb 

X  Position 

lib 

Y  Position 

Zb 

Z  Position 

4>b 

Roll  Angle 

9b 

Pitch  Angle 

i>b 

Yaw  Angle 

This  x,  y,  and  z  position  are  the  vector  from  the  original  position  of  the  body 
to  its  current  position.  The  body  frame  x-axis  points  from  the  center  of  the  body  out 
one  rotor.  In  the  case  of  a  quadrotor  the  nose  is  chosen  by  the  operator  and  defined 
as  shown  in  Figure  3.7(a).  The  y-axis  points  out  another  90  degrees  clockwise  when 
viewed  from  above,  and  the  z-axis  points  down  to  complete  a  right-handed  coordinate 
frame.  Figure  3.7(b)  defines  rotation  about  the  x  axis  the  roll  angle  0,  pitch  angle 
about  the  y  axis,  9,  and  yaw  is  about  the  z  axis,  % b. 

3.2.6  Quadrotor  Simulator.  To  provide  the  best  possible  results  the  most 
realistic  flight  trajectory  was  necessary  to  thoroughly  investigate  the  effects  of  the 
dynamics  of  a  flying  aircraft  on  an  onboard  sensor.  This  was  accomplished  through  the 
use  of  a  non  linear  quadrotor  simulator  developed  at  the  ANT  Center  [20] .  To  simulate 
a  trajectory  requires  four  command  inputs  for  the  duration  of  the  flight.  These  are  roll, 
pitch,  and  yaw  angles,  along  with  a  thrust  reference  for  vertical  movement.  Multiple 
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(a)  Quadrotor  Body  Frame  (b)  Quadrotor  Rotation  Angles 


Figure  3.7:  Quadrotor  Body  Frame  and  Attitude  Angles 

flight  profiles  were  produced  to  fly  around  the  ANT  center.  These  will  be  detailed  in 
the  next  chapter. 

A  typical  quadrotor  flying  with  a  heavy  payload,  such  as  the  360  degree  laser 
scanner,  will  use  angles  less  than  5  degrees  with  possible  aggressive  maneuvers  accom¬ 
plished  using  angles  up  to  approximately  10  degrees.  These  rotations  of  the  vehicle 
will  degrade  performance  of  the  laser  scan  measurements  as  detailed  in  Section  3.3.2. 

3.2.7  Generation  of  Scans  from  Flight  Profile.  The  realistic  flight  profiles  of 
the  quadrotor  platform  provide  the  capability  to  detect  the  effects  of  flight  dynamics 
on  laser  scanner  measurements.  As  previously  discussed,  the  calculations  generating 
laser  scans  require  different  reference  frames.  The  scanner  mounted  on  the  aircraft 
operates  in  the  body  frame,  but  the  algorithms  used,  (detailed  in  Section  3.3),  operate 
in  the  navigation  frame.  This  frame  is  chosen  to  be  an  East,  North,  Up  reference 
frame.  A  DCM  is  used  in  the  transformation  of  one  coordinate  frame  to  the  other. 
The  DCM  for  body  to  navigation  frame  is  shown  in  Equation  3.4 
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c?  = 

cos(^)  cos(d)  cos(,0)  sin(0)  sin  (0)  —  sia(ip)  cos(</>)  cos(V’)  sin(0)  cos(0)  +  sin  (ip)  sin( 
sin(,0)  cos(d)  sin(,0)  sin(0)  sin(0)  +  cos(ift)  cos(</>)  sin(^)  sin(0)  cos(</>)  —  cos(ip)  sin( 
—  sin(d)  cos(d)  sin(0)  cos(d)  cos(0) 

(3.4) 

where  (j)  is  the  roll  angle,  9  is  the  pitch  angle,  and  ^  is  the  yaw  angle. 

This  is  used  when  the  scanner  finds  the  range  of  points  in  which  it  would  detect 
and  find  a  range  measurement  represented  by  a  line  segment.  This  is  defined  in  the 
body  frame  as  the  point  of  the  scanner’s  position,  always  (0,0,0),  to  the  point  at  the 
range  of  the  sensor  when  rotated  according  the  vehicle’s  attitude.  To  convert  this  to 
the  nav  frame,  where  the  ANT  center  is  defined  and  measurement  occurs,  the  DCM  is 
first  evaluated  at  the  attitude  angles  of  the  aircraft,  then  multiplied  to  the  position  in 
the  body  frame,  and  added  to  the  vector  between  the  nav  and  body  frames  as  shown 
in  Equation  3.5. 


Xn 

xbl 

Xb0 

Yn 

=  c? 

Yb  i 

+ 

Yb  o 

Zn 

Zb\ 

- 1 

O 

N 

(3.5) 


where  Xn  is  the  end  point  of  the  laser  scan  in  the  navigation  frame,  Xb()  is  the  origin 
of  the  body  frame  and  A^is  the  end  point  of  the  scanner  measurement  in  the  body 
frame 


3.3  Histogram  Method  Calculation 

With  complete  scans  available  using  the  above  simulation,  a  method  is  neces¬ 
sary  to  extract  navigation  data.  One  of  the  motivations  of  the  histogram  correlation 
method,  introduced  by  Weiss  et  al.,  was  to  develop  a  navigation  method  completely 
independent  of  odometry  for  use  by  mobile  robots  [42] .  Another  motivation  was  that 
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it  be  fast  enough  that  it  could  be  used  to  generate  maps  in  real  time  so  that  accumu¬ 
lation  of  errors  would  be  reduced  when  returning  to  a  previously  mapped  location. 
There  were  two  possibilities  this  method  allowed  for  this  reduction  in  the  accumu¬ 
lation  of  errors.  First,  scans  that  were  meters  apart  are  still  able  to  be  compared 
accurately,  reducing  small  error  accumulation,  and  secondly,  it  was  possible  that  scan 
matching  could  correct  accumulated  error.  Adapting  these  ideas  to  navigating  in  a 
flying  environment,  where  an  IMU  is  used  instead  of  odometry,  made  this  method  a 
primary  investigation  for  a  standalone  navigation  solution  or  to  aid  onboard  systems. 

As  introduced  in  Section  2.4.5  the  scan  to  scan  correlation  using  the  histogram 
correlation  method  is  performed  in  three  steps.  First,  the  rotation  angle  between 
the  scans  is  found,  then  two  displacement  magnitudes  are  estimated.  This  method 
assumes  that  a  significant  portion  of  the  environment  is  the  same,  which  is  typically 
the  case  with  high  scan  rates  in  relation  to  the  vehicle  dynamics,  so  that  laser  scans 
can  be  matched  to  each  other.  If  inside  a  non  symmetrical  room,  a  laser  scan  from  any 
orientation  and  location  is  theoretically  correctly  matched  to  any  other  orientation  and 
location.  In  harsher  environments  additional  techniques  may  allow  correct  matching 
through  additional  knowledge  such  as  vehicle  dynamic  limits. 

The  angle  determination  fits  a  line  between  every  consecutive  point,  and  cal¬ 
culates  the  angle  of  this  line  and  the  scanner  axis.  Parallel  walls  generate  an  angle 
180  degrees  apart  and  are  often  combined  by  current  histogram  correlation  methods. 
A  visualization  of  how  this  angle  is  generated  is  shown  in  Figure  3.8.  These  angles 
are  then  placed  in  a  histogram  with  a  selected  bin  size,  which  is  equal  to  the  smallest 
distinguishable  rotation  determined  by  the  algorithm.  This  is  also  performed  to  the 
second  scan  and  the  results  placed  into  another  histogram.  With  no  noise  on  the  scan¬ 
ner,  and  no  translation  or  rotation,  these  will  be  identical.  If  there  is  only  a  rotation, 
these  histograms  will  be  identical  except  for  an  offset  of  the  angle.  A  translation  will 
produce  different  histograms,  but  with  the  assumptions  that  the  same  walls  are  visible 
they  can  still  be  compared  accurately.  To  determine  the  rotation  angle  between  the 
two  scans  a  discrete  circular  crosscorrelation  is  performed. 
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Measured  Data  Scan 


Figure  3.8:  Angle  Calculation  Used  in  Histogram  Algorithm 

The  maximum  of  this  correlation  directly  corresponds  to  the  rotation  angle 
9.  Depending  on  the  environment  and  sensor  capabilities,  0.1-3  degree  bins  have 
proven  to  provide  adequate  scan  matching.  Smaller  bins  benefit  when  there  is  minimal 
sensor  noise  and  an  environment  with  many  points  occupying  walls  facing  the  same 
directions.  This  results  in  enough  data  points  to  create  significant  spikes  in  histogram 
bins. 

Upon  determination  of  the  rotation  angle  the  scans  are  rotated  on  top  of  each 
other.  The  rotation  angle  determined  is  subtracted  from  the  angle  index  for  the 
second  scan  resulting  in  overlayed  scans  with  the  same  orientation.  Next,  both  scans 
are  rotated  so  that  the  maximum  angle  found  in  either  angle  histogram,  commonly 
referred  to  as  the  ’’main”  direction  of  the  scan  or  a,  is  placed  upon  the  x  axis. 
This  improves  determination  of  the  x  and  y  direction  translations  between  the  scans 
and  is  especially  useful  when  operating  in  a  man  made  environment  where  parallel 
and  perpendicular  walls  are  assumed  to  exist  in  the  environment.  In  this  thesis  the 
angle  histogram  was  transformed  into  a  90  degree  window  where  perpendicular  walls 
produce  the  same  angle.  The  maximum  of  this  was  used  to  rotate  the  scans  by  ±  45 
degrees. 

With  both  scans  rotated  to  the  same  orientation,  and  onto  the  x  and  y  axis, 
translation  was  determined  using  additional  histogram  correlations.  This  time,  the  x 
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and  y  distances  between  the  scanner  position  and  each  data  point  are  placed  into  a 
distance  histogram.  While  it  is  possible  to  use  a  histogram  ranging  in  size  between  the 
minimum  and  maximum  values  of  the  distances,  the  computational  requirement  would 
negate  the  purpose  of  the  algorithm.  The  distances  are  recalculated  using  the  modulo 
of  the  distance  and  the  window  size  selected,  setting  up  a  circular  correlation  to 
determine  translation  between  the  two  laser  scans.  Window  size  limits  the  magnitude 
of  the  translation  that  can  be  detected  to  half  of  the  window  size  because  of  the  nature 
of  the  correlation  performed.  Bin  size  is  selected  as  the  minimum  resolution  of  the 
translation  that  can  be  determined.  For  large  translations  between  scan  matches  it 
is  common  to  use  two  consecutive  histogram  correlations.  The  first  uses  meter  sized 
bins  to  capture  the  majority  of  the  translation,  which  is  followed  by  a  second  using 
a  smaller  bin  size  to  determine  the  remainder.  In  this  thesis  a  single  correlation  for 
each  direction  with  a  window  size  of  4  meters  and  bin  size  of  1  cm  allow  translation 
determination  up  to  2  meters  to  the  nearest  centimeter. 

The  histogram  correlation  algorithm  as  used  for  this  research  is  outlined  as 
follows: 

1.  Calculate  angle  histograms  of  both  scans 

2.  Correlate  first  and  second  scan  angle  histograms  to  determine  rotation  angle  9. 

3.  Determine  the  highest  occurring  angle  in  scan  1  (a). 

4.  Rotate  scan  1  by  a 

5.  Rotate  scan  2  by  a  +  9 

6.  Create  histogram  for  distance  of  each  point  to  the  x  axis  using  1  cm  bins 

7.  Correlate  for  A  x 

8.  Create  histogram  for  distance  of  each  point  to  the  y  axis  using  1  cm  bins 

9.  Correlate  for  A  y 

10.  Transform  body  translation  to  navigation  frame  by  using  the  sum  of  magnitudes 
in  each  of  the  x  and  y  directions  and  current  heading  angle  found  by  using 
previous  heading  +  a 

A  visual  representation  of  matching  two  scans  is  found  in  Figure  3.9.  Two  scans 
taken  in  the  ANT  Center  are  shown  in  the  top  left.  The  scanner  position  of  all  scans 
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in  the  body  frame  is  (0,0).  The  angle  correlation  calculates  a  heading  change  ( 9 )  of 
-4.1  degrees  which  is  added  to  the  second  scan  and  shown  in  the  top  right.  Bottom 
left  shows  the  scans  after  rotated  by  the  main  direction  (a).  The  algorithm  then 
calculates  a  Ax  of  10  cm,  and  a  Ay  of  56  cm.  The  successful  matching  is  shown  in 
the  bottom  right. 


Two  Measured  Scans  From  LD-OEMIOOO 


Scans  Rotated  to  Main  Direction 


Rotation  Angle  Determined 


Scans  After  Calculated  T ranslations 


Figure  3.9:  Histogram  Correlation  Method  Matching  Two  Scans  Taken  in  the  ANT 
Center 


This  algorithm  is  robust  to  elements  including  objects  moving  through  the  laser 
scanner  area  and  some  sensor  noise.  However,  it  does  not  always  determine  a  single 
value  for  the  maximum  correlation.  When  this  occurs  the  result  is  not  appropriate  to 
include  as  an  update.  Using  this  method  allows  the  next  scan  to  be  used  without  any 
significant  impact  in  performance  as  long  as  the  limits  are  not  surpassed,  (such  as  a 
translation  greater  than  two  meters  described  in  this  application).  This  may  cause 
stability  issues  if  a  poor  scan  is  not  handled  properly  and  cannot  be  matched  to  any 
future  scans.  Additional  challenges  to  the  algorithm  in  the  ANT  Center  environment 
include  the  interior  walls  are  not  straight  as  they  are  built  of  individual  sections  about 
three  feet  long.  These  walls  prohibit  the  LADAR  to  observe  the  entire  ANT  Center 
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at  anytime  from  any  position.  Also,  transitioning  into  the  hallway  and  back  produces 
a  significantly  different  viewable  environment.  The  algorithm  should  handle  these 
challenges  suitably. 


3.3.1  Extraction  of  Vehicle  Movement.  The  histogram  correlation  algorithm 
resolves  scan  to  scan  movement  in  the  body  frame,  as  a  heading  change,  and  an  x  and 
y  translation.  To  convert  this  into  the  navigation  frame  a  DCM  must  first  be  applied 
in  two  dimensions  shown  in  Equation  3.6. 


xn 

cos(-i/;)  sin(,0) 

xb 

Vn 

—  sin(,0)  cos(V’) 

Vb 

(3.6) 


The  vehicle  is  assumed  to  have  no  pitch  or  roll  angles  when  performing  this 
transformation  as  the  quadrotor’s  limited  pitch  and  roll  angles  can  use  the  small  angle 
approximation  where  the  cosine  of  small  angles  is  very  near  1.  For  example,  if  ignoring 
the  y  direction,  the  quadrotor  travels  10  cm  at  a  pitch  angle  of  2  degrees  the  true 
movement  is  9.994  cm  in  the  x  direction  and  0.35  cm  in  the  z  direction  as  detailed 
in  Figure  3.10.  Also,  the  algorithm  is  more  likely  to  estimate  incorrect  movement 
based  upon  skewing  range  measurements,  as  detailed  in  Section  3.3.2,  rather  than 
acquiring  error  due  to  these  small  cosine  effects.  The  calculated  x  and  y  vectors  in 
the  navigation  frame  are  added  to  the  previous  estimated  position  to  determine  the 
new  current  position. 


Current 

Position 


10  Cos(2)  =  9.994  cm 


10  Sin(2)  = 
0.35  cm 


Figure  3.10:  Quadrotor  Operation  uses  Small  Pitch  and  Roll  Angles  Allowing  the 
Application  of  the  Small  Angle  Approximation 
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This  histogram  correlation  method  uses  relative  positioning  meaning  any  head¬ 
ing  and  position  is  relative  to  its  first  known  (or  assumed)  state.  The  total  translations 
are  continually  integrated  over  time.  A  faster  update  rate  will  cause  an  increase  in 
the  accumulation  of  the  small  errors  to  grow  over  time,  but  too  slow  of  an  update 
rate  may  fail  when  the  environment  changes  too  quickly  such  as  passing  through  a 
doorway  with  limited  visibility  through  either  direction.  Updates  rates  of  10  Hz,  5  Hz, 
and  1  Hz  are  examined  in  both  simulated  and  experimental  trials. 

An  acquired  heading  error  will  continually  impact  the  x  and  y  errors  as  the 
calculated  values  will  not  coincide  with  the  x  and  y  axes  of  the  navigation  frame. 
Overall,  error  growth  of  this  method  can  be  attributed  primarily  to  rounding  errors 
and  incorrectly  selected  correlations,  which  are  caused  by  sensor  noise  or  environ¬ 
ment  anomalies,  and  also  constant  rounding  of  translations  and  angle  to  the  bin  size 
available. 

3.3.2  Flight  Attitude  Effect  On  Range  Measurement.  As  the  quadrotor 
maneuvers  in  flight,  the  distance  determined  by  the  scanner  changes  depending  on 
the  orientation  of  the  vehicle.  As  a  stationary  vehicle  rotates  either  direction  away 
from  horizontal  the  same  wall  will  appear  to  move  away  from  the  laser  scanner  as 
depicted  in  Figure  3.11.  This  effect  is  greater  the  further  from  a  wall,  and  the  larger 
the  angle  changes  between  scans  as  shown  in  Equation  3.7. 


True  Distance 

Measured  Distance  = - - - 

cos  0 


(3.7) 


The  effects  of  this  when  operating  in  the  assumed  environment  and  account¬ 
ing  for  the  limited  attitude  angles  during  quadrotor  operation  result  in  changes  of 
approximately  10  cm  in  the  worst  case.  These  effects  will  be  analyzed  in  Chapter  4 
comparing  two  and  three  dimensional  simulations.  When  an  onboard  INS  solution  is 
available  the  effects  of  this  can  significantly  reduced  as  a  measured  scan  can  be  scaled 
according  to  pitch  and  roll  angles. 
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True  Distance 


Figure  3.11:  Scanner  Measurements  are  Greater  than  or  Equal  to  the  True  Distance 
to  the  Wall 

3.3.3  Application  of  a  Smoothing  Filter.  When  the  histogram  method  was 
first  applied  to  both  the  simulated  and  truth  scan,  the  first  step  of  determining  rota¬ 
tion  angle  was  not  performing  well  to  the  desired  accuracy  of  0.1  degrees.  The  reason 
for  this  is  the  noise  associated  with  the  scanner  caused  the  angle  calculation  to  strug¬ 
gle.  Consecutive  points  on  walls  are  assumed  to  have  similar  angle,  but  as  shown  in 
Figure  3.12,  this  did  not  provide  the  ability  to  correlate  scans  as  the  angle  histogram 
would  have  a  maximum  of  3  or  4  hits  for  any  bin  of  0.1  degrees.  This  is  partially 
caused  by  operating  the  scanner  at  a  high  resolution  near  objects  where  many  mea¬ 
surements  occur  near  each  other.  A  moving  average  filter  was  applied  which  averaged 
between  1  and  20  consecutive  points  in  an  attempt  to  smooth  the  noise  effects  on  the 
scan  so  the  histogram  would  have  more  defined  peaks  where  the  walls  occur.  This 
method  would  be  ideal  for  an  environment  without  any  objects  except  for  the  walls. 
Through  this  method  the  effect  shown  in  Figure  3.12  is  improved  as  seen  in  another 
scan  found  in  Figure  3.13(a).  In  the  case  of  a  single  measurement  located  away  from  a 
wall  the  filter  will  not  smooth  noise,  but  instead  shift  the  group  of  points  first  toward 
the  object  and  then  back.  This  effect  is  detailed  in  Figure  3.13(b). 

Initial  determination  of  the  optimal  filter  length  observed  a  single  scan  at  a  time 
along  with  its  associated  angle  histogram.  Without  filtering  there  were  no  significant 
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Measured  Data  Scan 


Figure  3.12:  Laser  Scan  Produced  when  Operation  a  Laser  Scanner  Near  a  Wall 

spikes  which  would  cause  the  algorithm  difficulty  in  determining  the  correct  rotation 
between  two  scans.  Out  of  720  measurements  occupying  3600  bins,  the  maximum 
value  in  any  bin  was  3.  Increasing  the  filter  setting  made  individual  walls  visible. 
While  not  all  points  occupied  the  same  0.1  degree  bin,  there  were  a  few  consecutive 
bins  with  high  results.  In  the  ANT  Center  the  maximum  values  ranged  between  15 
and  20  depending  on  the  location  in  the  room  when  applying  a  filter  length  of  15. 
Increasing  the  filter  setting  beyond  20  reduced  the  spikes  as  significant  portions  of 
walls  were  mixing  with  adjacent  walls.  Additional  details  of  filter  setting  analysis  are 
found  in  Chapter  4. 

3.4  Line  Extraction 

Another  method  commonly  used  to  extract  information  from  laser  scans  or 
vision  cameras  is  through  the  use  of  a  line  extraction  algorithm.  Using  the  assumption 
that  the  laser  scanner  is  operating  in  a  man  made  environment  will  provide  many 
opportunities  of  straight  lines  to  be  found  in  the  laser  scan  whether  they  occur  inside 
a  room,  in  a  hallway,  or  even  the  exterior  of  buildings.  There  are  many  different 
extraction  methods  with  a  few  compared  using  a  SICK  LMS-291  by  Nguyen  et  ah  [31]. 
This  technique  is  most  often  applied  to  SLAM  applications  where  landmarks  are 
tracked  over  time. 
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Measured  Data  Scan 


(a)  Operation  of  the  laser  scanner  near  a  wall  pro¬ 
duces  many  points  where  angle  calculations  are  ir¬ 
relevant.  Smoothing  reduces  these  effects. 


Measured  Data  Scan 
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(b)  When  there  is  an  object  located  away  from 
the  wall,  the  wall  becomes  skewed.  This  effect  de¬ 
grades  performance  the  most  when  it  occurs  near 
a  corner. 

Figure  3.13:  Filtering  the  Scanner  Data  Provides  both  Good  and  Bad  Effects 


The  Hough  Transform  is  a  technique  whose  name  is  from  Paul  Hough  who 
patented  the  idea  in  1962  [13].  It  is  used  in  image  processing  to  detect  lines  through  a 
voting  process.  The  lines  are  represented  by  two  parameters,  a  distance  of  the  vector 
from  the  origin  normal  to  the  line,  p,  and  the  angle  of  this  vector  to  the  origin’s  axis, 
9,  as  shown  in  Figure  3.14.  Equation  3.8  represents  the  equation  of  a  line  in  Hough 
space  using  this  notation. 


x  cos (6)  +  y  sin(0)  =  p 


(3.8) 
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V 


Figure  3.14:  Hough  Space  representation:  p,  6 

This  technique  of  representing  lines  is  used  by  the  line  fitting  algorithm  used  by 
the  CAS  toolbox  for  MATLAB  [3].  This  toolbox  is  specifically  designed  for  use  with 
laser  scanner  range  data.  A  single  scan  where  line  extraction  is  performed  is  shown 
in  Figure  3.15.  This  algorithm  is  detailed  as  follows: 

1.  For  every  portion  of  points  using  a  sliding  window  across  the  range  readings 

(a)  Fit  line  model  to  points  within  window  using  least  squares  estimate 

(b)  Calculate  model  fidelity  measure 

(c)  If  model  fidelity  satisfies  condition,  create  a  segment  out  of  this  line 

2.  For  all  line  segments  found 

(a)  Check  if  each  pair  of  line  segments  are  collincar  and  satisfy  a  significance 
level  (a) 

(b)  Fnse  collincar  segments  into  a  line 

This  method  is  investigated  as  an  alternative  to  the  histogram  matching  algo¬ 
rithm.  Results  are  included  in  Chapter  4. 
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Figure  3.15: 
Center 


Local  map:  extracted  lines 
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IV.  Performance  Analysis 


This  chapter  reports  on  the  performance  of  the  algorithms  used  both  by  MATLAB 
simulation  as  well  as  processed  data  scans  using  collected  laser  scan  measure¬ 
ments  from  an  LD-OEM  1000  laser  scanner. 

4-1  Simulation  Results 

By  developing  a  simulation  to  model  quadrotor  flight,  the  resulting  laser  scans 
that  would  be  measured,  along  with  the  processing  of  these  scans,  the  various  effects 
of  each  step  can  be  isolated  to  measure  performance  characteristics.  Monte  Carlo 
simulations  of  each  setting  are  performed  to  observe  the  average  effects  over  many 
trial  runs.  A  number  of  25  was  selected  for  all  trials  to  capture  the  general  trends 
and  results  without  the  burden  of  extreme  computational  resources  and  time.  For 
each  trial  run  the  simulation  is  provided  with  the  perfect  scan  measurements  for  the 
selected  flight  profile  assuming  there  is  no  noise  on  the  measurement,  and  no  error  in 
the  angle  of  which  the  measurement  is  being  taken.  Noise  is  then  added  as  specified 
by  the  current  simulation  variable  setting.  The  measures  of  performance  for  each 
trial  are  the  error  in  the  x  and  y  direction  and  heading  angle.  Across  all  trials  the 
performance  is  evaluated  using  the  statistical  measures  of  the  mean  and  standard 
deviation  of  the  magnitude  in  the  X  and  Y  directions  along  with  the  heading  angle 
across  all  points  of  the  simulated  run. 

4-1.1  Flight  Profiles  Used  in  the  Simulation.  Various  flight  profiles  were 
generated  to  analyze  the  performance.  Profile  1  is  a  44  second  long  flight  generated 
as  a  possible  flight  trajectory  where  a  quadrotor  navigates  around  a  room,  then  returns 
to  the  original  location.  A  detailed  view  of  the  required  command  inputs  are  shown 
in  Figure  4.1(a)  and  resulting  profile  is  shown  in  Figure  4.1(b).  A  top  down  view  of 
this  as  flown  in  the  ANT  Center  is  shown  in  Figure  4.2(a).  Profile  2  is  generated  by 
taking  the  reverse  of  Profile  1.  The  last  step  of  Profile  1  is  identical  to  the  first  step 
of  Profile  2  and  the  opposite  trajectory  is  found.  This  assumption  is  valid  because  a 
pitch  used  in  Profile  1  to  slow  the  velocity  in  a  direction  becomes  a  pitch  to  increase 
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the  velocity  in  the  opposite  direction  of  Profile  2.  Also,  because  the  quadrotor  is 
near  stationary  at  the  beginning  and  end  these  can  be  viewed  as  independent  flights. 
Profile  3  is  identical  to  Profile  1,  but  shifted  3  meters  in  the  x  direction  so  that  the 
quadrotor  flies  into  the  hallway  before  returning  as  shown  in  Figure  4.2(b).  This 
aids  in  the  investigation  of  the  degradation  in  performance  of  the  vehicle  moving  to  a 
dissimilar  area.  Profile  4  is  the  opposite  of  Profile  3,  generated  using  the  same  method 
as  Profile  2. 

4-1-2  Simulated  Noise  Levels.  In  addition  to  changing  the  flight  profile,  the 
simulation  was  tested  across  various  noise  levels  on  the  scanner  measurement.  As 
shown  in  Section  3.2.3  the  expected  noise  level  of  the  scanner  was  to  be  a  Gaussian 
noise  around  the  true  distance  with  a  standard  deviation  of  3.8  cm.  A  noise  level  of  2 
cm,  and  no  noise,  were  all  processed  to  investigate  how  this  would  affect  the  position 
and  heading  calculation. 

4-1-3  Update  Rate  Analysis.  There  are  advantages  to  altering  the  update 
rate  of  this  algorithm.  A  slower  update  rate  benefits  in  that  the  small  accumulation 
of  errors  that  occur  every  time  data  scans  are  correlated  by  rounding  to  the  nearest 
centimeter  arc  minimized  slowing  the  error  growth  over  time.  However,  if  updates  per¬ 
formed  too  fast  and  the  vehicle  flies  through  a  doorway  and  experiences  a  dramatically 
different  scene  the  correlation  process  may  fail  resulting  in  an  erroneous  navigation 
solution.  The  simulation  was  tested  at  rates  of  10,  5  and  1  Hz  to  investigate  these 
hypotheses.  Figure  4.3  confirms  the  hypothesis  that  a  slower  update  rate  provides  a 
more  accurate  solution.  Both  1  and  5  Hz  perform  similar,  and  better  than  the  10  Hz 
and  their  simulation  runs  are  more  tightly  around  the  truth  as  shown  by  the  standard 
deviations.  Next,  Profile  3  was  used  to  investigate  performance  when  flying  through 
a  doorway.  These  results  are  shown  in  Figure  4.4  and  show  poor  performance  at  the 
1  Hz  rate.  The  errors  in  these  simulations  were  not  tied  to  traversing  the  doorway, 
but  occurred  at  8  seconds  into  the  profile  which  is  when  the  vehicle  traverses  through 
the  middle  of  the  room.  Although  the  position  was  not  correct,  the  heading  angle 
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still  performed  well  as  it  is  independent  of  the  translation.  Translation,  however,  does 
depend  on  an  accurate  heading. 

Figure  4.5  provides  details  into  each  run  of  the  Profile  3  simulation  when  scans 
are  processed  at  the  previously  mentioned  rate  of  1  Hz,  and  an  averaging  filter  size  of 
10.  The  performance  in  the  X  direction  is  very  poor  without  any  runs  determining  the 
correct  translation.  As  the  scans  are  matched  at  9  and  10  seconds  none  of  the  25  trials 
estimates  a  correct  translation  (within  rounding  error)  shown  by  the  significant  jump 
in  the  magnitude  of  the  error  in  all  cases.  However,  in  some  cases  the  Y  direction  is 
successfully  calculated. 
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(a)  Profile  1  Flight  Trajectory 


Time  (s) 


Time  (s) 


(b)  Profile  1  Commanded  Attitude  Angles 

Figure  4.1:  Profile  1  Simulation 
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(a)  Profile  1  Flight  Trajectory  as  shown  Inside  the 
ANT  Center 


(b)  Profile  3  Flight  Trajectory  as  shown  Inside  the 
ANT  Center 

Figure  4.2:  Top  Down  View  of  Simulation  Profiles  Inside  the  ANT  Center. 
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Figure  4.3:  Comparison  of  Update  Rates  for  Profile  1,  Noise  of  2  cm,  and  Filter 
Setting  of  10 
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Figure  4.4:  Comparison  of  Update  Rates  for  Profile  3,  Noise  of  2  cm,  and  Filter 
Setting  of  10 
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Figure  4.5: 
at  1  Hz 
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Monte  Carlo  simulations  showing  poor  performance  at  an  update  rate 
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4-1-4  Smoothing  Filter  Selection.  The  simulation  assumes  a  clutterless  en¬ 
vironment  where  only  walls  are  found  by  the  laser  scanner  as  previously  described  in 
Section  3.3.3.  While  some  filter  was  proven  to  be  necessary,  the  optimal  window  for  a 
moving  average  was  evaluated  using  multiple  techniques.  The  specific  profile  primar¬ 
ily  used  in  this  investigation  was  Profile  3,  at  a  noise  level  of  2  cm,  and  an  update  rate 
of  10  Hz.  These  errors  were  quantified  by  analyzing  the  statistics  of  the  X  and  Y  error 
and  standard  deviation  at  the  end  of  the  simulated  profile,  along  with  the  average  X 
and  Y  error  and  standard  deviation  across  the  entire  run.  A  selection  of  these  which 
changed  the  most  and  showed  differing  performance  are  shown  in  Figure  4.6. 
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Filter  Setting 


Figure  4.6:  Filter  Selection  Method  using  Statistics  Across  Profile  3  Simulation 

Trials 


Each  individual  trial  is  plotted  against  each  other  with  the  mean  errors  shown  in 
Figure  4.7.  The  low  filter  settings  tended  to  underestimate  the  X  direction  while  the 
rest  of  the  settings  performed  similarly.  In  the  Y  direction  the  low  settings  performed 
the  worst  with  the  rest  intermingled.  The  standard  deviation  results  provided  more 
useful  results  and  are  found  in  Figure  4.8.  For  both  directions  a  low  filter  setting 
results  in  a  large  standard  deviation  due  to  picking  up  a  heading  error  throughout  the 
run  which  greatly  impacts  the  algorithms  accuracy  to  compute  X  and  Y  translation 
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correctly  in  the  navigation  frame.  The  average  and  standard  deviation  of  errors 
in  heading  calculation  are  shown  in  Figure  4.9.  As  expected,  the  average  across 
Monte  Carlo  trials  perform  relatively  well,  however  the  standard  deviation  shows 
how  each  trial  results  in  a  significantly  different  navigation  solution.  To  compare 
Figures  4.10(a)  and  4.10(b)  show  the  25  estimated  paths  for  the  filter  settings  of  5, 
and  15.  Both  are  able  to  estimate  the  approximate  trajectory,  however  the  setting  of 
15  is  much  closer  to  truth. 

From  this  evaluation  in  simulation  it  is  estimated  that  the  best  filter  would  be 
at  a  setting  between  10  and  15.  Results  from  the  experimental  data  are  found  in 
Section  4.2. 

4-1-5  Comparison  of  2  and  3  Dimensional  Simulation.  By  comparing  two 
and  three  dimensional  simulations  the  immediate  degradation  in  performance  by  in¬ 
troducing  roll  and  pitch  angles  can  be  analyzed.  The  simulated  laser  scans  for  the 
two  dimensional  environment  used  only  the  x  and  y  position,  and  the  heading  of  the 
vehicle.  The  rest  of  the  vehicle  state  vector  was  ignored  and  the  scans  were  generated 
using  the  two  dimensional  line  segment  intersection  method.  For  the  three  dimen¬ 
sion  environment  the  other  vehicle  states:  roll,  pitch,  yaw,  and  altitude,  are  included 
causing  the  scans  to  skew  and  change  as  detailed  in  Section  3.3.2.  While  most  of  the 
simulated  profiles  showed  some  loss  in  performance  overall  results  showed  the  these 
effects  not  to  severely  hinder  the  performance  of  the  algorithm  in  this  setting.  A  few 
examples  of  the  comparison  between  2  and  3  dimensional  performance  are  shown. 
First,  Figure  4.11  is  the  performance  of  Profile  4,  with  a  noise  setting  of  4  cm,  pro¬ 
cessed  at  10  Hz,  and  a  filter  setting  of  15.  The  general  trends  in  error  growth  are 
very  similar  with  the  average  errors  often  coinciding.  However,  the  standard  deviation 
shows  that  the  3D  simulated  results  are  more  spread  out  than  the  2D  case,  especially 
towards  the  end  in  the  Y  direction.  Another  example  shown  is  Figure  4.12,  is  profile 
1,  with  a  noise  level  of  2  cm,  processed  at  10  Hz,  with  a  filter  setting  of  10.  Again 
these  perform  similarly  with  the  3D  having  a  larger  ending  standard  deviation. 
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Figure  4.7:  Mean  Error  in  X  and  Y  Direction 
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Figure  4.8:  Standard  Deviation  of  Error  in  X  and  Y  Direction 
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Figure  4.9: 


Heading  Mean  Error  and  Standard  Deviation 
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Time  (sec) 


(a)  Estimated  Trajectories  with  Filter  Setting  of  5 


(b)  Estimated  Trajectories  with  Filter  Setting  of  15 
Figure  4.10:  Filter  Setting 

4-1.6  Extended  Length  Simulation  Testing.  Characterization  of  the  error 
growth  using  the  histogram  correlation  algorithm  over  a  long  period  of  time  is  of 
great  interest.  Ideally,  bounded  growth  in  any  estimation  scheme  is  desired  as  op¬ 
eration  over  long  periods  of  time,  even  many  hours,  results  in  the  same  uncertainty 
of  measurements,  as  is  the  case  with  GPS.  Using  Profile  1  followed  by  Profile  3,  and 
again  repeating  Profiles  1  and  3,  created  a  simulation  of  four  times  the  length  of 
the  original  profiles.  By  comparing  the  error  growth  rates  in  this  situation  provides 
insight  into  overall  error  growth  using  this  method.  These  are  found  in  Figure  4.13 
and  show  the  results  of  processing  the  longer  profile  at  10  Hz,  at  a  noise  of  2  cm,  and 
a  filter  setting  of  15. 
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Figure  4.11:  2  D  and  3  D  Comparison  of  Simulation  Profile  4 


X  Direction 


Time  (seconds) 


Figure  4.12:  2  D  and  3  D  Comparison  of  Simulation  Profile  1 

These  results  indicate  that  performance  of  the  algorithm  is  trajectory  dependent. 
This  is  shown  by  the  periodic  growth  and  decay  of  the  standard  deviations  in  the 
errors  according  to  what  portion  of  the  profile  is  being  flown.  Overall,  the  average  X 
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Figure  4.13:  Error  Growth  of  A  Longer  Simulation  Profile 


position  error  after  3  minutes  and  56  seconds  was  determined  to  be  28.29  cm  with  a 
Y  position  error  of  42  cm.  This  is  a  total  positioning  error  of  50.1  cm.  The  average 
heading  angle  error  at  the  end  was  1.03  degrees.  Although  the  standard  deviations 
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of  all  errors  tend  to  be  influenced  by  the  vehicle  trajectory,  they  do  grow  over  time. 
An  approximate  numerical  analysis  of  the  error  growth  in  the  simulated  environment 
would  be  an  average  error  in  both  X  and  Y  direction  of  10  cm  per  minute  each, 
with  an  uncertainty  growth  of  ±15  cm  per  minute.  The  error  growth  in  heading  angle 
determination  is  one  degree  per  minute  with  an  uncertainty  of  ±2  degrees  per  minute. 

4-2  Experimental  Results 

4-2.1  Device  Setup.  Data  was  collected  in  the  ANT  Center  by  interfacing 
with  the  LD-OEM  1000  with  the  SICK  LIDAR  Matlab/C±±  Toolbox  [11]  either 
through  an  RS-232/422  serial  connection,  or  over  Ethernet.  The  LMS  series  of  SICK 
laser  scanners  use  serial  while  the  LD  series  uses  an  Ethernet  connection.  This  toolbox 
allows  setting  the  measurement  sector(s)  of  the  held  of  view  for  the  scanner,  angular 
resolution,  and  others,  but  the  driver  program  for  Windows  provided  by  SICK  was 
used  during  setup  of  the  device. 

The  LD-OEM  1000  provides  range  and  reflection  measurements,  time  tags,  and 
the  angles  at  which  the  measurements  were  taken  by  providing  the  start  and  end  angles 
along  with  the  angular  resolution.  A  collection  of  data  using  the  settings  used  in  these 
experiments  of  a  full  360  degree  scan,  at  a  resolution  of  one  half  degree,  and  at  a  rate  of 
10  hz.  Hundreds  of  consecutive  scans  were  saved  using  Matlab.  When  analyzing  this 
set  of  data  it  was  determined  that  there  were  dropped  frames  for  unknown  reasons. 
This  was  verified  by  looking  at  the  beginning  and  end  time  stamps  associated  with 
each  data  set  and  appeared  at  random  intervals  across  the  entire  data  collection. 
The  average  data  rate  of  the  collection  with  dropped  frames  was  between  9  and  9.5 
hz.  This  effect  is  shown  in  Figure  4.14.  Without  dropped  frames  each  scan  would 
start  every  100  ms,  but  gaps  of  200  ms  can  be  seen.  To  minimize  the  chance  of  this 
happening,  everything  to  reduce  the  computational  requirements  was  implemented  in 
attempts  to  resolve  this.  Even  without  collecting  reflection  measurements,  removal  of 
all  messages  displayed  in  the  Matlab  command  window,  and  preallocation  of  memory 
for  the  data  scans  had  no  affect  on  the  missing  measurements. 
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Figure  4.14:  Beginning  Time  Stamps  of  LADAR  Measured  Data  show  Dropped 

Frames 

While  the  goal  is  to  use  the  data  in  real  time,  the  data  scans  were  post  processed 
allowing  individual  datasets  to  be  processed  multiple  different  ways  to  compare  per¬ 
formance.  Update  rates  of  10  Hz,  2  Hz,  and  1  Hz  were  compared  by  using  every  scan 
for  a  10  Hz  update  rate,  or  in  the  case  of  2  Hz,  using  every  fifth  scan.  When  possible, 
the  time  tags  were  also  used  to  pick  out  the  appropriate  scans.  When  a  dropped 
frame  was  desired,  the  next  available  scan  was  used  instead.  These  data  scans  were 
also  processed  using  various  smoothing  filter  values  of  between  0  and  20.  Anything 
above  this  was  decided  to  be  unnecessary  and  likely  to  degrade  performance  as  large 
portions  of  true  walls  would  become  skewed. 

4-2.2  Realtime  Data  Display.  The  histogram  correlation  method  was  imple¬ 
mented  in  real  time  where  movement  in  the  body  frame  is  displayed  using  MATLAB. 
Doing  so  creates  an  easy  method  to  observe  certain  phenomena  such  as  people  walking 
through  the  room  or  testing  limits  to  the  rate  of  movement  necessary  to  be  detected. 
While  the  algorithm  is  fast  and  should  be  capable  of  running  at  the  10  Hz  scan  rate 
of  the  device,  this  was  not  quite  achieved.  It’s  possible  this  was  due  to  the  timing 
of  the  scans  being  available,  the  processing  time  involved,  non-optimized  code,  and 
the  overhead  of  outputting  the  results  to  the  display.  The  most  probable  reason  for 
not  achieving  10  Hz  is  the  small  bin  sizes  used  in  the  histogram  correlations  of  a 
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tenth  of  a  degree  for  angle,  and  1  cm  for  each  translation  bin.  This  contributed  to 
a  much  larger  processing  requirement  than  previously  demonstrated  by  others  using 
the  method  [42] .  An  update  rate  of  5  Hz  used  every  other  scan  of  the  device  operating 
at  10  Hz.  The  higher  scan  resolution  is  desired  as  interscan  movement  is  minimized 
which  degrades  the  scan  quality.  The  realtime  display  was  able  to  confirm  that  the 
algorithm  is  robust  when  people  walk  through  the  scanned  sector,  even  when  fairly 
near  to  the  scanner  obscuring  vast  portions  of  the  scan.  This  is  because  the  person 
only  distorts  a  portion  of  one  or  few  walls  in  a  scan.  The  remaining  portions  of  the 
wall  will  still  result  in  a  successful  correlation,  just  not  as  strong. 

Another  concern  when  applying  the  algorithm  to  real  scans  was  a  small  but 
constant  rate  of  change  in  heading.  This  was  investigated  as  a  possible  error  source 
because  the  scan  may  become  distorted  rather  than  the  algorithm  detecting  a  rotation 
change.  This  concern  was  not  observed  when  analyzed  with  realtime  data.  Conversely, 
the  x  and  y  translation  did  not  perform  as  well  when  introducing  a  similar  small,  but 
constant  movement.  To  overcome  this  a  reduced  bin  size  may  be  necessary.  However, 
this  error  source  is  platform  dependent  and  restricted  to  a  vehicle  expected  to  spend 
a  duration  time  in  a  hover.  This  will  not  be  a  primary  concern  during  the  operation 
of  this  method  on  a  quadrotor. 

4-2.3  Experimentation  using  Line  Extraction.  In  order  to  compare  perfor¬ 
mance  to  another  common  method,  the  line  extraction  method  detailed  in  Section  3.4 
was  implemented  using  true  data  scans  from  the  ANT  Center.  Extraction  of  rota¬ 
tion  and  translation  is  possible  using  this  method  by  using  a  search  window  for  each 
line  found  between  two  consecutive  scans.  By  using  the  angle  to  each  line,  and  the 
distance  to  the  point  normal  to  the  line,  rotation  and  movement  could  be  extracted 
similar  to  the  rotation  and  two  translation  idea  as  used  in  the  histogram  correlation 
method.  This  method  is  more  common  to  implement  when  using  SLAM. 

Two  consecutive  scans  taken  with  the  LD-OEM  1000  were  compared  to  inves¬ 
tigate  the  algorithms  performance.  The  resulting  lines  fit  to  the  scans  are  found  in 
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Figures  4.15(a)  and  4.15(b).  Each  segment  found  is  displayed  as  a  line  with  an  x  for 
endpoints.  These  segments  are  then  joined  into  lines  shown  by  the  numbered  and 
dashed  lines.  The  laser  scan  data  points  are  shown  with  a  single  gray  dot.  The  algo¬ 
rithm  performed  quite  well  using  default  line  fit  parameters  and  tolerances  of  joining 
segments  to  lines.  The  top  wall,  nearest  the  scanner  origin,  fits  a  single  line  to  3  line 
segments.  Most  of  the  exterior  walls  of  the  ANT  Center  were  fit  to  a  line,  along  with 
a  line  found  for  the  door  which  was  propped  open. 

Results  of  the  accuracy  of  this  method  provide  promising  results  for  use  in  nav¬ 
igation.  Hough  representation  of  corresponding  lines  in  these  scans  were  fit  within  an 
angle,  a,  of  0.2  degrees  and  distance,  r,  of  less  than  1  cm.  This  method  was  determined 
to  take  too  long,  requiring  over  a  half  second  processing  on  each  scan  when  using  a 
desktop  computer,  and  was  not  beneficial  for  the  purpose  of  an  onboard  immediate 
solution  so  it  was  not  investigated  further.  Computation  time  was  problematic  due  to 
the  amount  of  data  points  required  to  be  processed.  A  line  segment  was  fit  to  every 
consecutive  window  of  a  user  specified  number  of  points  before  fitting  these  segments 
into  lines.  For  this  method  to  perform  faster  the  resolution  of  the  scanner  could  be 
lowered  to  reduce  the  number  of  line  segments  fit  or  to  increase  processing  power. 

4-2-4  Data  Collection.  After  initial  interfacing  with  the  laser  scanner  and 
applying  the  basic  algorithms  to  experimental  data,  a  method  for  moving  the  device 
was  developed  to  investigate  performance.  The  device  was  mounted  on  a  moving 
cart,  shown  in  Figure  4.16.  This  was  rigidly  mounted  and  does  not  allow  for  sim¬ 
ulated  pitch  or  roll  angles,  but  it  provides  the  opportunity  to  use  real  data  from  a 
realistic,  yet  cluttered,  environment.  To  measure  performance,  a  truth  data  source, 
or  at  least  a  method  which  is  proven  to  provide  an  order  of  magnitude  better  mea¬ 
surements,  is  necessary.  Initially  a  Novatel  SPAN  IMU  HG1700  was  used  for  its  high 
rate  of  measurement  and  expected  accuracy  especially  over  such  a  short  test  run.  It 
was  initialized  using  dual  stationary  GPS  antennas,  however,  after  disconnecting  and 
traveling  paths  inside  the  ANT  Center  and  surrounding  hallways  the  position  solution 
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(b)  Scan  2 

Figure  4.15:  Two  Consecutive  Laser  Scans  in  the  ANT  Center  with  Extracted  Lines 

was  not  reliable.  The  IMU  was  taken  down  a  25  meter  hall  way  and  returned  to  start, 
but  was  off  by  more  than  5  meters  and  was  unable  to  be  used  in  any  comparison. 
Additional  processing  may  resolve  issues  such  as  misalignment  where  the  IMU  was 
not  sitting  horizontal  to  the  gravity  vector  resulting  in  drift  in  both  the  x  and  y  direc¬ 
tions.  A  significant  portion  of  this  should  be  accounted  for  during  the  initialization 
process,  so  it  was  determined  to  use  an  alternate  method  instead. 
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Figure  4.16:  Experiment  Setup  with  LD-OEM  1000  Laser  Scanner  Mounted  on 

Cart  with  Trimble  S6  Surveying  System  in  the  Foreground 

A  Trimble  S6  Total  Station  surveying  system,  also  shown  in  Figure  4.16,  was 
selected  as  an  alternative  method  for  comparison  to  provide  accurate  measurements 
within  4  mm  +  2  parts  per  million  [40].  While  these  measurements  are  suitable  for  a 
pseudo-truth  source,  timing  is  more  of  an  issue  with  this  system.  Measurements  are 
taken  every  second  with  the  precision  of  only  one  second.  This  was  determined  not  to 
be  a  limiting  factor  as  the  estimated  path  extracted  from  laser  scanner  data  was  most 
important.  To  align  the  Trimble  and  processed  laser  scan  data  a  stationary  vehicle 
for  a  period  of  time,  followed  by  movement,  would  allow  aligning  the  data  sources. 
Because  this  system  uses  line  of  sight  laser  measurements  it  is  not  possible  to  test 
every  trajectory  as  modeled  in  the  simulation.  The  ANT  center  has  a  location  to  view 
most  of  the  room,  however  it  was  not  possible  to  go  into  the  hallway  and  around  the 
corner  as  detailed  in  Section  4.1.1.  A  180  degree  view  of  the  ANT  Center  taken  from 
the  approximate  position  of  the  Trimble  system  is  shown  in  Figure  4.2.4. 

4-2.5  ANT  Center  Data  Collection  and  Results.  The  simulation  of  the  ANT 
Center  provided  an  estimate  of  performance  in  navigation  of  the  room  by  the  laser 


54 


Figure  4.17:  Panorama  of  the  ANT  Center  Showing  180  Field  of  View 


scanner  using  the  histogram  correlation  method.  While  the  simulation  only  modeled 
eight  flat  and  perpendicular  walls  there  are  actually  numerous  noisy  areas  of  the  room, 
such  as  shelves  placed  along  a  wall,  pipes  running  vertically  near  the  corners,  doors 
creating  a  different  wall  than  the  parallel  adjacent  wall,  and  also  the  temporary  walls 
which  were  not  straight,  or  parallel  to  existing  walls.  Also,  the  laser  scanner  moving 
on  the  cart  had  some  vibration  due  to  its  height  of  placement  so  the  cart  could  be 
easily  moved  without  obstructing  the  view  of  the  device.  Filtering  is  meant  to  reduce 
some  of  these  effects,  however,  degraded  performance  is  to  be  expected.  Side  by  side 
images  of  the  same  scan,  both  as  measured,  and  then  filtered  at  a  setting  of  15  are 
found  in  Figures  4.18(a)  and  4.18(b).  The  scans  are  taken  from  the  right  half  of 
the  room  with  the  door  visible  in  the  upper  right  corner  and  the  entrance  to  the 
temporary  walled  office  in  the  lower  left.  The  filtering  smooths  the  noisy  walls  along 
with  small  objects  that  were  found,  but  also  removes  other  features  such  as  the  entry 
way  to  the  office,  and  it’s  interior  wall. 


X  of  Body  Frame  (meters) 


(a)  Laser  Scan  from  the  ANT  Center  as  (b)  Laser  Scan  from  the  ANT  Center  Fil- 
Measured  tered 


Figure  4.18:  Identical  Scan  of  the  ANT  Center,  as  measured  and  filtered  at  a  setting 
of  15 
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Processing  of  the  collected  scans  were  processed  at  various  filter  settings  and 
update  rates.  As  previously  seen,  an  update  rate  of  10  Hz,  and  a  filter  of  15  provided 
the  most  stable  and  accurate  results.  The  ground  track  as  measured  by  the  Trimble 
survey  station,  along  with  the  estimated  trajectory  by  the  histogram  correlation  al¬ 
gorithm  are  shown  in  Figure  4.19.  The  resulting  x  and  y  errors  are  plotted  over  time 
in  Figure  4.20.  The  periodic  nature  of  these  errors  is  explained  in  Section  4.3.  Due  to 
the  restriction  of  the  Trimble  system  heading  information  is  unavailable.  Analysis  of 
the  measured  laser  scans  show  heading  variations  up  to  15  degrees,  and  observation 
of  the  estimated  heading  is  in  agreement  of  the  algorithm  tracking  the  heading  angle 
acceptably. 
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Figure  4.19:  Comparison  of  Trimble  Observed  and  Histogram  Correlation  Esti¬ 

mated  Trajectory 

Noticing  the  x  direction  has  an  error  growth  occurring  at  approximately  12 
seconds  with  constant  error  after  that  led  to  further  analysis  of  this  point.  Figure  4.21 
shows  the  interpolated  Trimble  data  points  and  the  estimates  as  provided  by  the 
histogram  correlation  method.  This  confirms  that  at  this  time  the  estimated  solution 
drifted  away  from  the  measurement,  but  then  remained  at  this  approximate  error. 
This  occurs  as  the  laser  scan  is  approximately  the  point  (3.5,5),  the  point  at  which 
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X  Direction  Error 


Figure  4.20:  Error  Growth  in  Each  Direction 

the  temporary  wall  in  the  x  direction  becomes  no  longer  visible.  This  was  a  point  of 
concern  for  use  of  the  algorithm  in  the  ANT  Center.  While  this  single  crossing  was 
noted  to  be  a  problem,  there  were  multiple  other  occurrences  where  this  was  not  an 
issue  as  the  vehicle  passed  near  this  same  point,  and  another  near  (12,5)  where  the 
same  effect  would  occur  in  this  direction. 


Figure  4.21:  Estimated  and  Reference  in  X  Direction  Over  Time 

Performance  in  the  y  direction,  as  shown  in  Figure  4.22,  shows  similar  behavior 
of  error  growth  which  happens  more  often  than  the  x  direction.  These  occur  when  the 
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scanner  passes  the  temporary  walls  which  lie  in  the  y  direction.  Due  to  the  geometry 
of  the  ANT  Center  the  laser  scanner  this  happens  twice  as  often  as  the  x  direction, 
and  performance  is  degraded  because  of  this. 


Figure  4.22:  Estimated  and  Reference  in  Y  Direction  Over  Time 

4-2.6  Hallway  Data  Collection  and  Results.  One  important  environment 
of  interest  in  navigation  of  the  vehicle  discussed  in  this  work  is  through  hallway  or 
corridor  navigation.  Concern  has  been  discussed  in  previous  applications  when  using 
the  correlation  method  in  a  long  hallway  due  to  issues  of  limited  visibility  of  the 
system.  A  laser  scanner  is  able  to  measure  numerous  points  in  the  walls  parallel 
to  it’s  movement  while  traversing  the  hallway  positioning  the  vehicle  between  these 
walls  well,  but  it  is  unable  to  measure  the  wall  perpendicular  providing  the  movement 
information  of  movement  down  the  hallway.  The  LD-OEM  1000  shows  improved 
results  due  to  zits  360  degree  held  of  view  allowing  measurement  of  an  additional  wall 
behind  the  vehicle,  and  also  its  higher  resolution  than  other  scanners  often  used  for 
this  method  resulting  in  additional  points  found  along  the  far  walls. 

While  these  benefits  are  expected  to  improve  performance,  navigation  in  the 
hallway  still  suffers  from  a  near/far  problem.  When  the  scanner  is  operated  less  than 
a  meter  from  2  walls  but  at  least  an  order  of  magnitude  more  from  the  others  the 
histogram  method  correlations  for  translation  will  not  perform  equally  well.  When 
filtered  properly  the  heading  angle  determination  should  perform  well  due  to  the 
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majority  of  laser  measurements  to  be  parallel  lines  that  are  easily  correlated.  An 
example  of  this  with  an  excellent  correlation  spike  is  found  in  Figure  4.23.  However, 
during  calculation  of  the  X  and  Y  directions  only  one  has  a  good  correlation  to  work 
from  as  seen  in  Figure  4.24  and  4.25.  Note:  the  X  and  Y  shown  here  do  not  correspond 
to  the  navigation  frame,  but  only  to  the  correlation  of  these  scans  as  they  are  rotated 
about  the  main  direction  angle  found  as  detailed  in  Section  3.3. 
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Figure  4.23:  Example  of  Angle  Correlation  while  Traveling  Down  a  Hallway 
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Figure  4.24:  Example  of  the  Bad  Direction  for  Translation  Correlation  as  a  Result 
of  Traveling  Down  a  Hallway 


Initial  testing  in  the  hallway  environment  was  performed  by  walking  the  cart 
around  all  four  hallways  adjacent  to  the  ANT  Center.  These  hallways  have  a  few 
peculiar  aspects  which  provide  additional  visibility  in  directions  perpendicular  to  the 
hallway  direction,  for  example  notches  for  drinking  fountains,  or  office  entryways. 
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Scan  1  Y  Histogram 
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Figure  4.25:  Example  of  the  Good  Direction  for  Translation  Correlation  as  a  Result 
of  Traveling  Down  a  Hallway 

A  sample  data  collect  processed  at  four  different  angle  filter  settings  is  found  in 
Figure  4.26.  As  expected  the  filter  settings  of  10  and  15  perform  the  best.  Analysis  of 


Figure  4.26:  Estimated  Path  around  Hallway  using  Different  Filter  Settings 


these  results  produced  a  few  interesting  anomalies.  At  one  portion  of  the  first  hallway 
traveled  the  scans  estimated  a  backwards  movement  when  a  fairly  consistent  forward 
motion  was  used  for  the  duration  of  the  hallway.  This  coincides  with  a  portion  of  the 
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hallway  where  there  was  minimal  information  in  the  other  direction.  For  most  of  the 
movement  throughout  the  hallways  there  were  open  doors  allowing  visibility  of  walls 
perpendicular  to  the  hallway  which  aided  in  determining  the  lengthwise  direction. 
The  estimated  backwards  motion  is  shown  in  Figure  4.27(a)  and  is  present  in  all  filter 
settings  as  only  the  angle  correlation  calculation  uses  filtered  data  scans.  The  x  and 
y  translation  calculations  should  be  identical,  but  applied  differently  based  upon  the 
current  estimated  heading  angle.  This  erroneous  correlation  occurs  when  there  is 
minimal  information  in  the  one  direction  as  shown  by  one  corresponding  scan  taken 
in  the  area  in  Figure  4.27(b).  This  flawed  translation  calculation  was  noticed  across 
many  trials  on  collected  on  different  days  all  occuring  in  the  same  location. 


(a)  Incorrect  Translation  Extracted  from  Data  (b)  Laser  Scan  of  the  Hallway 
Scan  Measurements  with  Insufficient  Datapoints  to 

Extract  Movement  Down  the 
Hallway 

Figure  4.27:  Poor  Performance  of  the  Algorithm  While  Traveling  Down  a  Hallway 


4-3  Comparison  of  Simulated  and  Experimental  Data 

To  compare  experimental  data  to  the  simulation  data  a  similar  trajectory  was 
traveled  using  the  cart,  as  compared  in  Figure  4.28.  The  simulated  profile  travels  a 
similar  trajectory  in  80  seconds,  while  the  experimental  data  travels  across  the  room 
four  times  in  63  seconds. 
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Figure  4.28:  Simulated  and  Experimental  Trajectory  as  Traveled  in  the  ANT  Center 

The  periodic  nature  of  the  error  growth  of  the  experimental  data  is  due  to 
the  limitations  in  the  Trimble  measurement  system.  To  analyze  error  growth  over 
time  the  accurate  one  second  timestamps  were  assumed  to  happen  at  exactly  a  1  Hz 
rate,  and  points  were  linearly  interpolated  between  the  two  points  according  to  the 
time  stamp  of  the  rangefinder’s  data  scan.  This  effect  creates  a  jagged  flight  profile 
estimation  where  the  curves  become  straightened.  The  periodic  growth  and  decay 
of  this  is  detailed  in  Figure  4.31  where  the  vehicle’s  path  first  travels  away  from  the 
estimated  path  for  a  half  second,  and  then  back  toward  the  estimated  path  for  the 
next  half  second. 

While  Section  3.2.3  suggested  that  the  simulation  noise  level  of  2  cm  was  similar 
to  actual  performance  of  the  laser  scanner  the  results  more  similarly  matched  the  noise 
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Figure  4.29:  Comparison  using  2  cm  noise  estimate 
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Figure  4.30:  Comparison  using  4  cm  noise  estimate 


level  expected  by  the  LD-1000  data  sheet  of  4  cm.  Each  representative  plot  is  shown 
in  Figures  4.29  and  Figures  4.30. 


63 


Error  in  X  Direction 


Figure  4.31:  Periodic  Line  Fit  Error  Caused  by  Interpolation  of  Trimble  Data 

There  were  many  factors  contributing  to  degraded  performance  in  the  experi¬ 
mental  setup  that  were  not  modeled  in  the  simulation.  These  include  the  vibration 
in  the  scanner  when  mounted  high  on  the  cart,  temporary  walls  which  aren’t  perpen¬ 
dicular  or  parallel,  additional  pipes  and  shelves  in  the  environment  degrading  scan 
performance,  and  the  introduction  of  heading  changes  while  the  cart  was  moving. 
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V.  Conclusions  and  Recommendations 


This  chapter  overviews  the  achieved  performance  of  the  developed  navigation  so¬ 
lution  using  the  histogram  correlation  algorithm  method.  Suggested  improve¬ 
ments  and  relevant  future  testing  experiments  are  also  discussed. 

5.1  Conclusion  of  Performance 

5.1.1  Simulation.  The  MATLAB  simulation  developed  for  this  work  takes 
advantage  of  a  realistic  quadrotor  simulator  to  provide  realistic  flight  and  therefore 
accurate  performance  expectations  for  any  environment.  It  is  easily  modified  to  assist 
in  analysis  of  the  effects  of  update  rate,  sensor  noise,  and  flight  trajectory.  Investiga¬ 
tion  into  other  variables  such  as  the  use  of  a  different  laser  scanner  are  also  possible. 
This  framework  is  able  to  predict  performance  in  a  specific  situation.  With  a  high- 
fidelity  environment  model  and  predicted  flight  plan  tuning  parameters  such  as  the 
size  of  the  moving  average  filter  can  be  adjusted  for  the  optimal  unaided  navigation 
solution. 

In  simulation  an  update  rate  of  10  Hz  was  stable  for  all  flight  profiles  tested, 
but  performed  worse  than  an  update  rate  of  5  Hz  or  1  Hz  for  most  instances  of  the 
simulation.  Often,  the  1  Hz  trials  acquired  errors  that  would  not  be  acceptable  for 
providing  a  navigation  position  solution.  Rates  slower  than  this  are  possible  when 
operating  in  a  limited  environment,  but  they  were  not  experimented  with  in  this 
work.  If  the  accuracy  of  the  solution  when  using  a  10  Hz  update  rate  was  prohibitive 
a  simulation  may  assist  in  selecting  the  appropriate  update  rate  for  a  given  flight 
trajectory  and  environment. 

5.1.2  Real  World.  The  algorithms  adapted  for  use  in  this  thesis  were  only 
tested  in  a  two  dimensional  environment  due  to  the  test  setup  available.  Performance 
in  this  setting  was  evaluated  in  two  distinct  environments  of  interest,  inside  a  moder¬ 
ately  sized  room,  and  in  long  hallways.  Using  appropriate  tuning  parameters  for  each 
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case  yielded  a  valuable  navigation  solution  in  both  cases.  These  specific  values  would 
need  to  be  determined  either  through  simulation  or  experimentation  for  each  case. 

Overall  the  most  stable  results  with  experimental  data  occurred  when  processing 
at  an  update  rate  of  10  Hz  with  a  filter  setting  of  15.  To  improve  performance  these 
may  be  tweaked  depending  on  the  environment.  For  better  results  the  update  rate 
can  be  lowered,  yet  this  may  cause  the  solution  to  become  unstable.  With  additional 
processing  and  system  modeling  this  may  be  avoidable,  or  at  least  acknowledged  by 
the  algorithm  in  real  time. 

In  conclusion,  the  histogram  correlation  algorithm  applied  in  this  work  was 
shown  to  successfully  navigate  a  realistic  environment  where  a  quadrotor  may  be  uti¬ 
lized.  The  current  calculated  position  and  heading  solution  provided  by  the  algorithm 
is  acceptable  for  vehicles  using  short  flight  times  of  less  than  5  min  in  open  areas. 
Application  of  the  algorithm  in  hallway  navigation  shows  great  promise  providing 
a  stable  heading  determination  along  with  tracking  movement  perpendicular  to  the 
hallway.  However,  additional  modifications  may  be  necessary  to  assist  in  the  transla¬ 
tion  parallel  to  the  hallway.  The  results  shown  in  this  thesis  show  promising  potential 
in  the  use  of  using  a  LADAR  to  provide  a  position  and  heading  solution  over  longer 
flights.  This  may  be  accomplished  with  additional  processing  or  by  coupling  laser 
measurements  with  an  additional  sensor. 

5.2  Suggested  Improvements  to  the  Current  Algorithm 

5.2.1  Additional  Filtering  of  Laser  Scan  Data.  Filtering  laser  scan  measure¬ 
ments  when  performing  the  rotation  angle  calculations  proved  to  be  beneficial,  even 
necessary,  to  provide  a  stable  solution.  A  similar  filtering  method  may  improve  trans¬ 
lation  correlations.  The  histogram  correlation  method  tends  to  suffer  from  a  near  far 
problem  where  many  points  are  generated  when  a  scanner  is  in  closer  proximity  to 
a  single  wall  than  any  others.  The  number  of  points  found  on  this  wall  may  greatly 
outnumber  the  points  measured  in  the  rest  of  the  environment.  This  effect  is  most 
apparent  in  a  hallway  environment.  Additional  filtering  methods  may  include  deter- 
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mining  the  proximity  of  consecutive  laser  scan  measurements  to  generate  a  balanced 
number  of  data  points  along  each  wall,  and  each  direction. 

5.2.2  Integration  with  Additional  Hardware  or  Software.  As  with  most 
sensors  used  in  navigation  they  are  often  combined  to  provide  a  better  overall  estimate 
using  a  type  of  Kalman  Filter.  Combining  the  laser  scanner  detailed  in  this  thesis 
with  a  MEMs  IMU  would  provide  significant  improvement  as  each  sensor  behaves 
differently.  The  IMU  has  a  fast  update  rate  where  the  laser  scanner  is  slower  and 
would  be  capable  of  limiting  a  portion  of  the  drift  in  the  IMU.  One  instance  of 
this  would  be  when  traveling  in  the  hallway  environment  and  the  independent  laser 
scanner  solution  estimated  reverse  movement,  as  discussed  in  Section  4.2.6.  This  could 
be  avoided  because  the  system  model  used  in  the  Kalman  filter  would  understand  this 
movement  to  be  impossible  by  the  vehicle,  especially  with  input  from  an  IMU,  and  put 
little  weight  into  factoring  in  the  measurement  update.  Additional  aiding  techniques 
may  be  applied  using  a  Kalman  filter  without  additional  sensors  and  rely  on  the 
system  model  to  weigh  updates  appropriately. 

An  alternative  method  of  using  an  IMU  to  aid  the  developed  method  would  be 
to  correct  the  scans  according  to  the  vehicle’s  attitude  so  that  a  significant  portion 
of  the  skewing  behavior  would  be  mitigated.  This  would  not  be  as  effective  for  this 
application  using  the  quadrotor  where  attitude  angles  of  less  than  eight  degrees  are 
expected  during  operation  in  relatively  small  environments.  If  a  vehicle  with  more 
extreme  flight  dynamics,  or  operation  in  a  larger  environment  such  as  outdoor  in  a 
city  street,  the  performance  may  be  increased  when  performing  these  corrections. 

5.2.3  Testing  in  a  3D  Environment.  Ultimately  this  work  was  developed 
to  operate  on  a  quadrotor  platform  and  would  greatly  benefit  from  flight  testing. 
Performing  such  tests  in  the  ANT  Center  where  a  Vicon  motion  capture  system 
can  provide  real  time  true  position  and  vehicle  attitude  would  be  ideal.  This  was  not 
pursued  because  the  quadrotor  in  development  to  fly  the  LADAR  unit  is  not  currently 
flying.  Other  experimentation  using  the  motion  tracking  was  not  performed  because 
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of  the  limited  translation  available  to  stay  within  the  system’s  field  of  view.  Under 
this  setup  truth  data  is  available  at  a  much  higher  rate  than  the  Trimble  surveying 
system  which  is  also  limited  to  positioning  information. 

Using  the  immediate  attitude  information  available  by  this  system  could  provide 
a  more  accurate  framework  for  the  investigation  into  direct  aiding  of  the  laser  scan 
data  correcting  for  pitch  and  roll  angles.  This  would  assist  in  quantifying  the  benefit 
of  coupling  an  IMU  or  other  device  during  flight. 

5.2.4  Future  Topics  of  Interest.  Due  to  limited  experimental  data  and 
simulations  which  were  too  similar,  the  error  growth  necessary  when  implementing  a 
Kalman  filter,  was  not  clearly  identified.  The  error  growth  was  shown  to  be  strongly 
influenced  by  the  environment  and  also  by  the  flight  trajectory.  This  was  witnessed 
in  the  long  simulation  detailed  in  the  end  of  chapter  4.  A  better  characterization  of 
the  error  growth  is  recommended. 
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stable  heading  along  with  tracking  movement  perpendicular  to  the  hallway. _ 
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